Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixes an error in the OBB orientation calculation #100

Merged
merged 1 commit into from
Jun 2, 2018
Merged

Fixes an error in the OBB orientation calculation #100

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions releaseNotes.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,11 @@
----------------------------------------------------------------
Release 0.9.3
???? ??, 2018

Bug fixes
OBBShape had inconsistent definitions for getXBasis() and getYBasis().
Documentation clarified, usage unified.

----------------------------------------------------------------
Release 0.9.2
May 28, 2018
Expand Down
79 changes: 52 additions & 27 deletions src/Menge/MengeCore/Math/Geometry2D.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -584,10 +584,13 @@ namespace Menge {
// First rotate so that the OBB is an AABB then use the same logic as
// with the AABB
Vector2 disp = q - _pivot;
// the LOCAL x- and y- coordinates of the nearest point, initialized to the
// local value of the query point.
float X = disp.x() * _cosTheta + disp.y() * _sinTheta;
float Y = disp.y() * _cosTheta - disp.x() * _sinTheta;
// Rotation from world to geometry: R = [Bx By]^T.
Vector2 Bx = getXBasis();
Vector2 By = getYBasis();
// the LOCAL x- and y- coordinates of the nearest point, initialized to the
// local value of the query point.
float X = Bx * disp;
float Y = By * disp;

// based on the voronoi regions of the AABB
// (-1,1) | (0,1) | (1,1)
Expand Down Expand Up @@ -646,24 +649,22 @@ namespace Menge {
}

Vector2 localPt(X, Y);
Vector2 rot0 = getXBasis();
Vector2 rot1 = getYBasis();
Vector2 targetPt(_pivot + Vector2(localPt * rot0, localPt * rot1));
directions.setTarget(targetPt);
Vector2 prefDir(norm(targetPt - q));
if (dimensions) {
// there is actually a span
localPt.set(xL, yL);
Vector2 leftPt(_pivot + Vector2(localPt * rot0, localPt * rot1));
localPt.set(xR, yR);
Vector2 rightPt(_pivot + Vector2(localPt * rot0, localPt * rot1));
directions.setSpan(norm(leftPt - q),
norm(rightPt - q),
prefDir);
}
else {
directions.setSingle(prefDir);
}
// Rotation from geometry to world is: R = [Bx, By]
Vector2 rot0(Bx._x, By._x);
Vector2 rot1(Bx._y, By._y);
Vector2 targetPt(_pivot + Vector2(localPt * rot0, localPt * rot1));
directions.setTarget(targetPt);
Vector2 prefDir(norm(targetPt - q));
if (dimensions) {
// there is actually a span
localPt.set(xL, yL);
Vector2 leftPt(_pivot + Vector2(localPt * rot0, localPt * rot1));
localPt.set(xR, yR);
Vector2 rightPt(_pivot + Vector2(localPt * rot0, localPt * rot1));
directions.setSpan(norm(leftPt - q), norm(rightPt - q), prefDir);
} else {
directions.setSingle(prefDir);
}
}
}

Expand All @@ -673,8 +674,13 @@ namespace Menge {
// First rotate so that the OBB is an AABB then use the same logic as
// with the AABB
Vector2 disp = q - _pivot;
float X = disp.x() * _cosTheta + disp.y() * _sinTheta;
float Y = disp.y() * _cosTheta - disp.x() * _sinTheta;
// Rotation from world to geometry: R = [Bx By]^T.
Vector2 Bx = getXBasis();
Vector2 By = getYBasis();
// the LOCAL x- and y- coordinates of the nearest point, initialized to the
// local value of the query point.
float X = Bx * disp;
float Y = By * disp;

// based on the voronoi regions of the AABB
// 0 | 1 | 2
Expand Down Expand Up @@ -717,8 +723,11 @@ namespace Menge {
}

Vector2 localPt(X, Y);

return _pivot + Vector2(localPt * getXBasis(), localPt * getYBasis());
// Rotation from geometry to world is: R = [Bx, By]
Vector2 rot0(Bx._x, By._x);
Vector2 rot1(Bx._y, By._y);

return _pivot + Vector2(localPt * rot0, localPt * rot1);
}

/////////////////////////////////////////////////////////////////////
Expand All @@ -727,7 +736,23 @@ namespace Menge {
return _pivot + getXBasis() * (_size.x() * 0.5f) + getYBasis() * (_size.y() * 0.5f);
}

/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////

Vector2 OBBShape::convertToWorld(const Vector2& r_GP) const {
Vector2 Bx = getXBasis();
Vector2 By = getYBasis();
Vector2 r_GP_W(r_GP * Vector2(Bx._x, By._x), r_GP * Vector2(Bx._y, By._y));
return _pivot + r_GP_W;
}

/////////////////////////////////////////////////////////////////////

Vector2 OBBShape::convertToGeometry(const Vector2& r_WP) const {
Vector2 r_GP_W = r_WP - _pivot;
return Vector2(r_GP_W * getXBasis(), r_GP_W * getYBasis());
}

/////////////////////////////////////////////////////////////////////
// Implementation of XML Parsing
/////////////////////////////////////////////////////////////////////

Expand Down
130 changes: 117 additions & 13 deletions src/Menge/MengeCore/Math/Geometry2D.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -747,19 +747,123 @@ namespace Menge {
*/
virtual Vector2 getCentroid() const;

/*!
* @brief Returns the x-axis of the OBB's local frame
*
* @returns The direction of the obb's x-axis.
*/
Vector2 getXBasis() const { return Vector2(_cosTheta, -_sinTheta); }

/*!
* @brief Returns the y-axis of the OBB's local frame
*
* @returns The direction of the obb's y-axis.
*/
Vector2 getYBasis() const { return Vector2(_sinTheta, _cosTheta); }
/*!
@brief Converts a position vector from the geometry origin to P, expressed in the geometry
frame (`r_GP`) to a position vector from the world origin to P, expressed in the
world frame (`r_WP`).

@param r_GP The position vector from the geometry origin to a point P, expressed in the
geometry frame.
@retval r_WP_W The position vector from world origin to the point P, expressed in the world
frame.
*/
Vector2 convertToWorld(const Vector2& r_GP) const;

/*!
@brief Converts a position vector from the world origin to P, expressed in the world frame
(`r_WP`) to a position vector from the geometry origin to P, expressed in the
geometry frame (`r_GP`).

@param r_WP The position vector from the world origin to a point P, expressed in the world
frame.
@retval r_GP_G The position vector from geometry origin to the point P, expressed in the
geometry frame.
*/
Vector2 convertToGeometry(const Vector2& r_WP) const;

/*!
@brief Returns the x-axis of the OBB's local frame expressed in the world frame.

If we say that Bx = getXBasis() and By = getYBasis(), are *column* vectors, we can define the
2x2 matrix R_WG = [Bx By] - a rotation taking vectors expressed in the geometry frame and
expressing them in the world frame.

For example, the position vector from the pivot to the far corner, expressed in the geometry
frame would be `r_PC_G = [w h]^T`. We can express that same vector in the world frame as:
`r_PC_W = R_WG * r_PC_G`.

Conversely, the matrix `R_GW = R_WG^T = [Bx By]^T` re-expresses in the *geometry* frame a
vector that was previously expressed in the world frame.

In practice, for a position vector from the world origin to point Q expressed in the world
frame, r_WQ_W, we can calculate a position vector from the geometry frame's origin r_WG (the
OBB's pivot) and expressed in the geometry frame as:
`r_GQ = [(r_WQ - r_WG) * Bx, (r_WQ - r_WG) * By]^T` or
`r_GQ = R_GW * (r_WQ - r_WG)`.

Functionally, that becomes:

```
Vector2 r_WQ_W;
Vector2 Bx = getXBasis();
Vector2 By = getYBasis();
Vector2 r_GQ_W = r_WQ - _pivot;
Vector2 r_GQ_G(r_GQ_W * Bx, r_GQ_W * By);
```

Mapping the opposite direction is a bit trickier (in the absence of a 2x2 matrix).

```
Vector2 r_GQ_G;
Vector2 Bx = getXBasis();
Vector2 By = getYBasis();
Vector2 r_GQ_W(r_GQ_G * Vector2(Bx._x, By._x), r_GQ_G * Vector2(Bx._y, By._y));
Vector2 r_WQ_W = _pivot + r_GQ_W;
```

See [Drake's monogram notation](http://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html)
for details on how to interpret the symbols, `r_WQ_W`, `R_WG`, etc.

@returns The direction of the obb's x-axis expressed in the world frame.
*/
Vector2 getXBasis() const { return Vector2(_cosTheta, _sinTheta); }

/*!
@brief Returns the y-axis of the OBB's local frame expressed in the world frame

If we say that Bx = getXBasis() and By = getYBasis(), are *column* vectors, we can define the
2x2 matrix R_WG = [Bx By] - a rotation taking vectors expressed in the geometry frame and
expressing them in the world frame.

For example, the position vector from the pivot to the far corner, expressed in the geometry
frame would be `r_PC_G = [w h]^T`. We can express that same vector in the world frame as:
`r_PC_W = R_WG * r_PC_G`.

Conversely, the matrix `R_GW = R_WG^T = [Bx By]^T` re-expresses in the *geometry* frame a
vector that was previously expressed in the world frame.

In practice, for a position vector from the world origin to point Q expressed in the world
frame, r_WQ_W, we can calculate a position vector from the geometry frame's origin r_WG (the
OBB's pivot) and expressed in the geometry frame as:
`r_GQ = [(r_WQ - r_WG) * Bx, (r_WQ - r_WG) * By]^T` or
`r_GQ = R_GW * (r_WQ - r_WG)`.

Functionally, that becomes:

```
Vector2 r_WQ_W;
Vector2 r_GQ_W = r_WQ - _pivot;
Vector2 Bx = getXBasis();
Vector2 By = getYBasis();
Vector2 r_GQ_G(r_GQ_W * Bx, r_GQ_W * By);
```

Mapping the opposite direction is a bit trickier (in the absence of a 2x2 matrix).

```
Vector2 r_GQ_G;
Vector2 Bx = getXBasis();
Vector2 By = getYBasis();
Vector2 r_GQ_W(r_GQ_G * Vector2(Bx._x, By._x), r_GQ_G * Vector2(Bx._y, By._y));
Vector2 r_WQ_W = _pivot + r_GQ_W;
```

See [Drake's monogram notation](http://drake.mit.edu/doxygen_cxx/group__multibody__notation__basics.html)
for details on how to interpret the symbols, `r_WQ_W`, `R_WG`, etc.

@returns The direction of the obb's y-axis expressed in the world frame.
*/
Vector2 getYBasis() const { return Vector2(-_sinTheta, _cosTheta); }

/*!
* @brief Returns the size of the obb (w, h)
Expand Down
32 changes: 20 additions & 12 deletions src/Menge/MengeVis/Runtime/GoalRenderer/OBBGoalRenderer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,29 +30,37 @@ namespace MengeVis {
_goal->getStringId() + " with OBB goal renderer." );
}
const OBBShape * obb = static_cast<const OBBShape *>( goal->getGeometry() );
Vector2 X = obb->getXBasis();
Vector2 Y = obb->getYBasis();
Vector2 size = obb->getSize();
Vector2 pivot = obb->getPivot();

Vector2 c( size.x(), 0.f );
Vector2 c1( c * X, c * Y );
// Compute the four corners in the world frame:
//
// c3 ___________ c2
// | |
// |___________|
// O c1
//
// O = <0, 0, 0>
// c1 = <w, 0, 0>
// c2 = <w, h, 0>
// c3 = <0, h, 0>
//
Vector2 c( 0.f, 0.f );
Vector2 c0 = obb->convertToWorld(c);
c.set(size.x(), 0.f);
Vector2 c1 = obb->convertToWorld(c);
c.set( size );
Vector2 c2( c * X, c * Y );
Vector2 c2 = obb->convertToWorld(c);
c.set( 0.f, size.y() );
Vector2 c3( c * X, c * Y );
Vector2 c3 = obb->convertToWorld(c);

glPushMatrix();
glTranslatef( pivot.x(), pivot.y(), 0.f );
glBegin( GL_POLYGON );
glVertex3f( 0.f, 0.f, 0.f );
glVertex3f( c0.x(), c0.y(), 0.f );
glVertex3f( c1.x(), c1.y(), 0.f );
glVertex3f( c2.x(), c2.y(), 0.f );
glVertex3f( c3.x(), c3.y(), 0.f );
glVertex3f( 0.f, 0.f, 0.f );
glEnd();
glPopMatrix();
}
} // namespace GoalVis
} // namespace Runtime
} // namespace MengeVis
} // namespace MengeVis