diff --git a/releaseNotes.txt b/releaseNotes.txt index 7f43d93e..87c0d1b9 100644 --- a/releaseNotes.txt +++ b/releaseNotes.txt @@ -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 diff --git a/src/Menge/MengeCore/Math/Geometry2D.cpp b/src/Menge/MengeCore/Math/Geometry2D.cpp index 3f5dc672..b34afd7d 100644 --- a/src/Menge/MengeCore/Math/Geometry2D.cpp +++ b/src/Menge/MengeCore/Math/Geometry2D.cpp @@ -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) @@ -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); + } } } @@ -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 @@ -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); } ///////////////////////////////////////////////////////////////////// @@ -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 ///////////////////////////////////////////////////////////////////// diff --git a/src/Menge/MengeCore/Math/Geometry2D.h b/src/Menge/MengeCore/Math/Geometry2D.h index 7661773c..118f6995 100644 --- a/src/Menge/MengeCore/Math/Geometry2D.h +++ b/src/Menge/MengeCore/Math/Geometry2D.h @@ -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) diff --git a/src/Menge/MengeVis/Runtime/GoalRenderer/OBBGoalRenderer.cpp b/src/Menge/MengeVis/Runtime/GoalRenderer/OBBGoalRenderer.cpp index dd7447ee..a2bca37d 100644 --- a/src/Menge/MengeVis/Runtime/GoalRenderer/OBBGoalRenderer.cpp +++ b/src/Menge/MengeVis/Runtime/GoalRenderer/OBBGoalRenderer.cpp @@ -30,29 +30,37 @@ namespace MengeVis { _goal->getStringId() + " with OBB goal renderer." ); } const OBBShape * obb = static_cast( 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 = + // c2 = + // 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 \ No newline at end of file +} // namespace MengeVis