Simbody
3.5
|
00001 #ifndef SimTK_SIMMATH_CONTACT_GEOMETRY_H_ 00002 #define SimTK_SIMMATH_CONTACT_GEOMETRY_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * Simbody(tm): SimTKmath * 00006 * -------------------------------------------------------------------------- * 00007 * This is part of the SimTK biosimulation toolkit originating from * 00008 * Simbios, the NIH National Center for Physics-Based Simulation of * 00009 * Biological Structures at Stanford, funded under the NIH Roadmap for * 00010 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 00011 * * 00012 * Portions copyright (c) 2008-12 Stanford University and the Authors. * 00013 * Authors: Peter Eastman, Michael Sherman * 00014 * Contributors: Ian Stavness, Andreas Scholz * 00015 * * 00016 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 00017 * not use this file except in compliance with the License. You may obtain a * 00018 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 00019 * * 00020 * Unless required by applicable law or agreed to in writing, software * 00021 * distributed under the License is distributed on an "AS IS" BASIS, * 00022 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 00023 * See the License for the specific language governing permissions and * 00024 * limitations under the License. * 00025 * -------------------------------------------------------------------------- */ 00026 00031 #include "SimTKcommon.h" 00032 #include "simmath/internal/common.h" 00033 #include "simmath/internal/OrientedBoundingBox.h" 00034 #include "simmath/internal/Geodesic.h" 00035 #include "simmath/internal/BicubicSurface.h" 00036 00037 #include <cassert> 00038 00039 namespace SimTK { 00040 00044 SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactGeometryTypeId); 00045 00046 class ContactGeometryImpl; 00047 class OBBTreeNodeImpl; 00048 class OBBTree; 00049 class Plane; 00050 00051 00052 00053 //============================================================================== 00054 // CONTACT GEOMETRY 00055 //============================================================================== 00110 class SimTK_SIMMATH_EXPORT ContactGeometry { 00111 public: 00112 class HalfSpace; 00113 class Sphere; 00114 class Ellipsoid; 00115 class Torus; 00116 class SmoothHeightMap; 00117 class Cylinder; 00118 class Brick; 00119 class TriangleMesh; 00120 00121 // TODO 00122 class Cone; 00123 00125 ContactGeometry() : impl(0) {} 00127 ContactGeometry(const ContactGeometry& src); 00129 ContactGeometry& operator=(const ContactGeometry& src); 00133 ~ContactGeometry(); 00134 00136 DecorativeGeometry createDecorativeGeometry() const; 00137 00148 Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const; 00149 00164 Vec3 projectDownhillToNearestPoint(const Vec3& pointQ) const; 00165 00230 bool trackSeparationFromLine(const Vec3& pointOnLine, 00231 const UnitVec3& directionOfLine, 00232 const Vec3& startingGuessForClosestPoint, 00233 Vec3& newClosestPointOnSurface, 00234 Vec3& closestPointOnLine, 00235 Real& height) const; 00236 00237 00238 00250 bool intersectsRay(const Vec3& origin, const UnitVec3& direction, 00251 Real& distance, UnitVec3& normal) const; 00252 00258 void getBoundingSphere(Vec3& center, Real& radius) const; 00259 00263 bool isSmooth() const; 00264 00281 void calcCurvature(const Vec3& point, Vec2& curvature, 00282 Rotation& orientation) const; 00283 00294 const Function& getImplicitFunction() const; 00295 00301 Real calcSurfaceValue(const Vec3& point) const; 00302 00313 UnitVec3 calcSurfaceUnitNormal(const Vec3& point) const; 00314 00320 Vec3 calcSurfaceGradient(const Vec3& point) const; 00321 00327 Mat33 calcSurfaceHessian(const Vec3& point) const; 00328 00357 Real calcGaussianCurvature(const Vec3& gradient, 00358 const Mat33& Hessian) const; 00359 00363 Real calcGaussianCurvature(const Vec3& point) const { 00364 return calcGaussianCurvature(calcSurfaceGradient(point), 00365 calcSurfaceHessian(point)); 00366 } 00367 00376 Real calcSurfaceCurvatureInDirection(const Vec3& point, const UnitVec3& direction) const; 00377 00386 void calcSurfacePrincipalCurvatures(const Vec3& point, Vec2& curvature, 00387 Rotation& R_SP) const; 00388 00391 bool isConvex() const; 00392 00398 Vec3 calcSupportPoint(UnitVec3 direction) const; 00399 00402 ContactGeometryTypeId getTypeId() const; 00403 00454 static Vec2 evalParametricCurvature(const Vec3& P, const UnitVec3& nn, 00455 const Vec3& dPdu, const Vec3& dPdv, 00456 const Vec3& d2Pdu2, const Vec3& d2Pdv2, 00457 const Vec3& d2Pdudv, 00458 Transform& X_EP); 00459 00533 static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1, 00534 const UnitVec3& x2, const Vec2& k2, 00535 Rotation& R_SP, Vec2& k); 00536 00541 static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1, 00542 const UnitVec3& x2, const Vec2& k2, 00543 Vec2& k); 00544 00545 00559 void initGeodesic(const Vec3& xP, const Vec3& xQ, const Vec3& xSP, 00560 const GeodesicOptions& options, Geodesic& geod) const; 00561 00562 00605 // XXX if xP and xQ are the exact end-points of prevGeod; then geod = prevGeod; 00606 void continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod, 00607 const GeodesicOptions& options, Geodesic& geod) const; 00608 00635 void makeStraightLineGeodesic(const Vec3& xP, const Vec3& xQ, 00636 const UnitVec3& defaultDirectionIfNeeded, 00637 const GeodesicOptions& options, Geodesic& geod) const; 00638 00639 00649 // XXX what to do if tP is not in the tangent plane at P -- project it? 00650 void shootGeodesicInDirectionUntilLengthReached 00651 (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength, 00652 const GeodesicOptions& options, Geodesic& geod) const; 00653 00667 void calcGeodesicReverseSensitivity 00668 (Geodesic& geodesic, 00669 const Vec2& initSensitivity = Vec2(0,1)) const; // j, jdot at end point 00670 00671 00681 // XXX what to do if tP is not in the tangent plane at P -- project it? 00682 // XXX what to do if we don't hit the plane 00683 void shootGeodesicInDirectionUntilPlaneHit(const Vec3& xP, const UnitVec3& tP, 00684 const Plane& terminatingPlane, const GeodesicOptions& options, 00685 Geodesic& geod) const; 00686 00687 00690 void calcGeodesic(const Vec3& xP, const Vec3& xQ, 00691 const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const; 00692 00695 void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ, 00696 const Vec3& tPhint, Real lengthHint, Geodesic& geod) const; 00697 00700 void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ, 00701 Geodesic& geod) const 00702 { 00703 const Vec3 r_PQ = xQ - xP; 00704 const Real lengthHint = r_PQ.norm(); 00705 const UnitVec3 n = calcSurfaceUnitNormal(xP); 00706 // Project r_PQ into the tangent plane. 00707 const Vec3 t_PQ = r_PQ - (~r_PQ*n)*n; 00708 const Real tLength = t_PQ.norm(); 00709 const UnitVec3 tPhint = 00710 tLength != 0 ? UnitVec3(t_PQ/tLength, true) 00711 : n.perp(); // some arbitrary perpendicular to n 00712 calcGeodesicUsingOrthogonalMethod(xP, xQ, Vec3(tPhint), lengthHint, geod); 00713 } 00714 00715 00723 Vec2 calcSplitGeodError(const Vec3& P, const Vec3& Q, 00724 const UnitVec3& tP, const UnitVec3& tQ, 00725 Geodesic* geod=0) const; 00726 00727 00728 00739 // XXX what to do if tP is not in the tangent plane at P -- project it? 00740 void shootGeodesicInDirectionUntilLengthReachedAnalytical 00741 (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength, 00742 const GeodesicOptions& options, Geodesic& geod) const; 00743 00744 00755 // XXX what to do if tP is not in the tangent plane at P -- project it? 00756 // XXX what to do if we don't hit the plane 00757 void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP, 00758 const Plane& terminatingPlane, const GeodesicOptions& options, 00759 Geodesic& geod) const; 00760 00761 00766 void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ, 00767 const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const; 00768 00777 Vec2 calcSplitGeodErrorAnalytical(const Vec3& P, const Vec3& Q, 00778 const UnitVec3& tP, const UnitVec3& tQ, 00779 Geodesic* geod=0) const; 00780 00791 const Plane& getPlane() const; 00794 void setPlane(const Plane& plane) const; 00796 const Geodesic& getGeodP() const; 00798 const Geodesic& getGeodQ() const; 00799 const int getNumGeodesicsShot() const; 00800 void addVizReporter(ScheduledEventReporter* reporter) const; 00805 explicit ContactGeometry(ContactGeometryImpl* impl); 00806 bool isOwnerHandle() const; 00807 bool isEmptyHandle() const; 00808 bool hasImpl() const {return impl != 0;} 00810 const ContactGeometryImpl& getImpl() const {assert(impl); return *impl;} 00812 ContactGeometryImpl& updImpl() {assert(impl); return *impl; } 00813 00814 protected: 00815 ContactGeometryImpl* impl; 00816 }; 00817 00818 00819 00820 //============================================================================== 00821 // HALF SPACE 00822 //============================================================================== 00826 class SimTK_SIMMATH_EXPORT ContactGeometry::HalfSpace : public ContactGeometry { 00827 public: 00833 HalfSpace(); 00834 00836 UnitVec3 getNormal() const; 00837 00839 static bool isInstance(const ContactGeometry& geo) 00840 { return geo.getTypeId()==classTypeId(); } 00842 static const HalfSpace& getAs(const ContactGeometry& geo) 00843 { assert(isInstance(geo)); return static_cast<const HalfSpace&>(geo); } 00845 static HalfSpace& updAs(ContactGeometry& geo) 00846 { assert(isInstance(geo)); return static_cast<HalfSpace&>(geo); } 00847 00849 static ContactGeometryTypeId classTypeId(); 00850 00851 class Impl; 00852 const Impl& getImpl() const; 00853 Impl& updImpl(); 00854 }; 00855 00856 00857 00858 //============================================================================== 00859 // CYLINDER 00860 //============================================================================== 00864 class SimTK_SIMMATH_EXPORT ContactGeometry::Cylinder : public ContactGeometry { 00865 public: 00866 explicit Cylinder(Real radius); 00867 Real getRadius() const; 00868 void setRadius(Real radius); 00869 00871 static bool isInstance(const ContactGeometry& geo) 00872 { return geo.getTypeId()==classTypeId(); } 00874 static const Cylinder& getAs(const ContactGeometry& geo) 00875 { assert(isInstance(geo)); return static_cast<const Cylinder&>(geo); } 00877 static Cylinder& updAs(ContactGeometry& geo) 00878 { assert(isInstance(geo)); return static_cast<Cylinder&>(geo); } 00879 00881 static ContactGeometryTypeId classTypeId(); 00882 00883 class Impl; 00884 const Impl& getImpl() const; 00885 Impl& updImpl(); 00886 }; 00887 00888 00889 00890 //============================================================================== 00891 // SPHERE 00892 //============================================================================== 00895 class SimTK_SIMMATH_EXPORT ContactGeometry::Sphere : public ContactGeometry { 00896 public: 00897 explicit Sphere(Real radius); 00898 Real getRadius() const; 00899 void setRadius(Real radius); 00900 00902 static bool isInstance(const ContactGeometry& geo) 00903 { return geo.getTypeId()==classTypeId(); } 00905 static const Sphere& getAs(const ContactGeometry& geo) 00906 { assert(isInstance(geo)); return static_cast<const Sphere&>(geo); } 00908 static Sphere& updAs(ContactGeometry& geo) 00909 { assert(isInstance(geo)); return static_cast<Sphere&>(geo); } 00910 00912 static ContactGeometryTypeId classTypeId(); 00913 00914 class Impl; 00915 const Impl& getImpl() const; 00916 Impl& updImpl(); 00917 }; 00918 00919 00920 00921 //============================================================================== 00922 // ELLIPSOID 00923 //============================================================================== 00945 class SimTK_SIMMATH_EXPORT ContactGeometry::Ellipsoid : public ContactGeometry { 00946 public: 00950 explicit Ellipsoid(const Vec3& radii); 00953 const Vec3& getRadii() const; 00959 void setRadii(const Vec3& radii); 00960 00966 const Vec3& getCurvatures() const; 00967 00980 UnitVec3 findUnitNormalAtPoint(const Vec3& P) const; 00981 00989 Vec3 findPointWithThisUnitNormal(const UnitVec3& n) const; 00990 00999 Vec3 findPointInSameDirection(const Vec3& Q) const; 01000 01023 void findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const; 01024 01030 void findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& n, 01031 Transform& X_EP, Vec2& k) const; 01032 01034 static bool isInstance(const ContactGeometry& geo) 01035 { return geo.getTypeId()==classTypeId(); } 01037 static const Ellipsoid& getAs(const ContactGeometry& geo) 01038 { assert(isInstance(geo)); return static_cast<const Ellipsoid&>(geo); } 01040 static Ellipsoid& updAs(ContactGeometry& geo) 01041 { assert(isInstance(geo)); return static_cast<Ellipsoid&>(geo); } 01042 01044 static ContactGeometryTypeId classTypeId(); 01045 01046 class Impl; 01047 const Impl& getImpl() const; 01048 Impl& updImpl(); 01049 }; 01050 01051 01052 01053 //============================================================================== 01054 // SMOOTH HEIGHT MAP 01055 //============================================================================== 01066 class SimTK_SIMMATH_EXPORT 01067 ContactGeometry::SmoothHeightMap : public ContactGeometry { 01068 public: 01072 explicit SmoothHeightMap(const BicubicSurface& surface); 01073 01076 const BicubicSurface& getBicubicSurface() const; 01077 01080 const OBBTree& getOBBTree() const; 01081 01083 static bool isInstance(const ContactGeometry& geo) 01084 { return geo.getTypeId()==classTypeId(); } 01086 static const SmoothHeightMap& getAs(const ContactGeometry& geo) 01087 { assert(isInstance(geo)); return static_cast<const SmoothHeightMap&>(geo); } 01089 static SmoothHeightMap& updAs(ContactGeometry& geo) 01090 { assert(isInstance(geo)); return static_cast<SmoothHeightMap&>(geo); } 01091 01093 static ContactGeometryTypeId classTypeId(); 01094 01095 class Impl; 01096 const Impl& getImpl() const; 01097 Impl& updImpl(); 01098 }; 01099 01100 01101 //============================================================================== 01102 // BRICK 01103 //============================================================================== 01106 class SimTK_SIMMATH_EXPORT ContactGeometry::Brick : public ContactGeometry { 01107 public: 01110 explicit Brick(const Vec3& halfLengths); 01114 const Vec3& getHalfLengths() const; 01116 void setHalfLengths(const Vec3& halfLengths); 01117 01119 const Geo::Box& getGeoBox() const; 01120 01122 static bool isInstance(const ContactGeometry& geo) 01123 { return geo.getTypeId()==classTypeId(); } 01125 static const Brick& getAs(const ContactGeometry& geo) 01126 { assert(isInstance(geo)); return static_cast<const Brick&>(geo); } 01128 static Brick& updAs(ContactGeometry& geo) 01129 { assert(isInstance(geo)); return static_cast<Brick&>(geo); } 01130 01132 static ContactGeometryTypeId classTypeId(); 01133 01134 class Impl; 01135 const Impl& getImpl() const; 01136 Impl& updImpl(); 01137 }; 01138 01139 01140 //============================================================================== 01141 // TRIANGLE MESH 01142 //============================================================================== 01163 class SimTK_SIMMATH_EXPORT ContactGeometry::TriangleMesh 01164 : public ContactGeometry { 01165 public: 01166 class OBBTreeNode; 01177 TriangleMesh(const ArrayViewConst_<Vec3>& vertices, const ArrayViewConst_<int>& faceIndices, bool smooth=false); 01186 explicit TriangleMesh(const PolygonalMesh& mesh, bool smooth=false); 01188 int getNumEdges() const; 01190 int getNumFaces() const; 01192 int getNumVertices() const; 01196 const Vec3& getVertexPosition(int index) const; 01202 int getFaceEdge(int face, int edge) const; 01207 int getFaceVertex(int face, int vertex) const; 01212 int getEdgeFace(int edge, int face) const; 01217 int getEdgeVertex(int edge, int vertex) const; 01222 void findVertexEdges(int vertex, Array_<int>& edges) const; 01225 const UnitVec3& getFaceNormal(int face) const; 01228 Real getFaceArea(int face) const; 01234 Vec3 findPoint(int face, const Vec2& uv) const; 01239 Vec3 findCentroid(int face) const; 01244 UnitVec3 findNormalAtPoint(int face, const Vec2& uv) const; 01255 Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const; 01268 Vec3 findNearestPoint(const Vec3& position, bool& inside, int& face, Vec2& uv) const; 01269 01279 Vec3 findNearestPointToFace(const Vec3& position, int face, Vec2& uv) const; 01280 01281 01293 bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const; 01307 bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, int& face, Vec2& uv) const; 01310 OBBTreeNode getOBBTreeNode() const; 01311 01314 PolygonalMesh createPolygonalMesh() const; 01315 01317 static bool isInstance(const ContactGeometry& geo) 01318 { return geo.getTypeId()==classTypeId(); } 01320 static const TriangleMesh& getAs(const ContactGeometry& geo) 01321 { assert(isInstance(geo)); return static_cast<const TriangleMesh&>(geo); } 01323 static TriangleMesh& updAs(ContactGeometry& geo) 01324 { assert(isInstance(geo)); return static_cast<TriangleMesh&>(geo); } 01325 01327 static ContactGeometryTypeId classTypeId(); 01328 01329 class Impl; 01330 const Impl& getImpl() const; 01331 Impl& updImpl(); 01332 }; 01333 01334 01335 01336 //============================================================================== 01337 // TRIANGLE MESH :: OBB TREE NODE 01338 //============================================================================== 01343 class SimTK_SIMMATH_EXPORT ContactGeometry::TriangleMesh::OBBTreeNode { 01344 public: 01345 OBBTreeNode(const OBBTreeNodeImpl& impl); 01348 const OrientedBoundingBox& getBounds() const; 01350 bool isLeafNode() const; 01353 const OBBTreeNode getFirstChildNode() const; 01356 const OBBTreeNode getSecondChildNode() const; 01359 const Array_<int>& getTriangles() const; 01363 int getNumTriangles() const; 01364 01365 private: 01366 const OBBTreeNodeImpl* impl; 01367 }; 01368 01369 //============================================================================== 01370 // TORUS 01371 //============================================================================== 01377 class SimTK_SIMMATH_EXPORT ContactGeometry::Torus : public ContactGeometry { 01378 public: 01379 Torus(Real torusRadius, Real tubeRadius); 01380 Real getTorusRadius() const; 01381 void setTorusRadius(Real radius); 01382 Real getTubeRadius() const; 01383 void setTubeRadius(Real radius); 01384 01386 static bool isInstance(const ContactGeometry& geo) 01387 { return geo.getTypeId()==classTypeId(); } 01389 static const Torus& getAs(const ContactGeometry& geo) 01390 { assert(isInstance(geo)); return static_cast<const Torus&>(geo); } 01392 static Torus& updAs(ContactGeometry& geo) 01393 { assert(isInstance(geo)); return static_cast<Torus&>(geo); } 01394 01396 static ContactGeometryTypeId classTypeId(); 01397 01398 class Impl; 01399 const Impl& getImpl() const; 01400 Impl& updImpl(); 01401 }; 01402 01403 01404 01405 01406 //============================================================================== 01407 // GEODESIC EVALUATOR helper classes 01408 //============================================================================== 01409 01410 01414 class Plane { 01415 public: 01416 Plane() : m_normal(1,0,0), m_offset(0) { } 01417 Plane(const Vec3& normal, const Real& offset) 01418 : m_normal(normal), m_offset(offset) { } 01419 01420 Real getDistance(const Vec3& pt) const { 01421 return ~m_normal*pt - m_offset; 01422 } 01423 01424 Vec3 getNormal() const { 01425 return m_normal; 01426 } 01427 01428 Real getOffset() const { 01429 return m_offset; 01430 } 01431 01432 private: 01433 Vec3 m_normal; 01434 Real m_offset; 01435 }; // class Plane 01436 01437 01442 class GeodHitPlaneEvent : public TriggeredEventHandler { 01443 public: 01444 GeodHitPlaneEvent() 01445 : TriggeredEventHandler(Stage::Position) { } 01446 01447 explicit GeodHitPlaneEvent(const Plane& aplane) 01448 : TriggeredEventHandler(Stage::Position) { 01449 plane = aplane; 01450 } 01451 01452 // event is triggered if distance of geodesic endpoint to plane is zero 01453 Real getValue(const State& state) const { 01454 if (!enabled) { 01455 return 1; 01456 } 01457 Vec3 endpt(&state.getQ()[0]); 01458 Real dist = plane.getDistance(endpt); 01459 // std::cout << "dist = " << dist << std::endl; 01460 return dist; 01461 } 01462 01463 // This method is called whenever this event occurs. 01464 void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const { 01465 if (!enabled) { 01466 return; 01467 } 01468 01469 // This should be triggered when geodesic endpoint to plane is zero. 01470 Vec3 endpt; 01471 const Vector& q = state.getQ(); 01472 endpt[0] = q[0]; endpt[1] = q[1]; endpt[2] = q[2]; 01473 Real dist = plane.getDistance(endpt); 01474 01475 // ASSERT(std::abs(dist) < 0.01 ); 01476 shouldTerminate = true; 01477 // std::cout << "hit plane!" << std::endl; 01478 } 01479 01480 void setPlane(const Plane& aplane) const { 01481 plane = aplane; 01482 } 01483 01484 const Plane& getPlane() const { 01485 return plane; 01486 } 01487 01488 const void setEnabled(bool enabledFlag) { 01489 enabled = enabledFlag; 01490 } 01491 01492 const bool isEnabled() { 01493 return enabled; 01494 } 01495 01496 private: 01497 mutable Plane plane; 01498 bool enabled; 01499 01500 }; // class GeodHitPlaneEvent 01501 01505 class PathDecorator : public DecorationGenerator { 01506 public: 01507 PathDecorator(const Vector& x, const Vec3& O, const Vec3& I, const Vec3& color) : 01508 m_x(x), m_O(O), m_I(I), m_color(color) { } 01509 01510 virtual void generateDecorations(const State& state, 01511 Array_<DecorativeGeometry>& geometry) { 01512 // m_system.realize(state, Stage::Position); 01513 01514 Vec3 P, Q; 01515 P[0] = m_x[0]; P[1] = m_x[1]; P[2] = m_x[2]; 01516 Q[0] = m_x[3]; Q[1] = m_x[4]; Q[2] = m_x[5]; 01517 01518 geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_O)); 01519 geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(P)); 01520 geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(Q)); 01521 geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_I)); 01522 01523 geometry.push_back(DecorativeLine(m_O,P) 01524 .setColor(m_color) 01525 .setLineThickness(2)); 01526 geometry.push_back(DecorativeLine(Q,m_I) 01527 .setColor(m_color) 01528 .setLineThickness(2)); 01529 01530 } 01531 01532 private: 01533 const Vector& m_x; // x = ~[P Q] 01534 const Vec3& m_O; 01535 const Vec3& m_I; 01536 const Vec3& m_color; 01537 Rotation R_plane; 01538 Vec3 offset; 01539 }; // class DecorationGenerator 01540 01541 01545 class PlaneDecorator : public DecorationGenerator { 01546 public: 01547 PlaneDecorator(const Plane& plane, const Vec3& color) : 01548 m_plane(plane), m_color(color) { } 01549 01550 virtual void generateDecorations(const State& state, 01551 Array_<DecorativeGeometry>& geometry) { 01552 // m_system.realize(state, Stage::Position); 01553 01554 // draw plane 01555 R_plane.setRotationFromOneAxis(UnitVec3(m_plane.getNormal()), 01556 CoordinateAxis::XCoordinateAxis()); 01557 offset = 0; 01558 offset[0] = m_plane.getOffset(); 01559 geometry.push_back( 01560 DecorativeBrick(Vec3(Real(.01),1,1)) 01561 .setTransform(Transform(R_plane, R_plane*offset)) 01562 .setColor(m_color) 01563 .setOpacity(Real(.2))); 01564 } 01565 01566 private: 01567 const Plane& m_plane; 01568 const Vec3& m_color; 01569 Rotation R_plane; 01570 Vec3 offset; 01571 }; // class DecorationGenerator 01572 01573 01574 } // namespace SimTK 01575 01576 #endif // SimTK_SIMMATH_CONTACT_GEOMETRY_H_