Simbody  3.5
ContactGeometry.h
Go to the documentation of this file.
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_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines