Simbody  3.5
ContactGeometry.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_CONTACT_GEOMETRY_H_
2 #define SimTK_SIMMATH_CONTACT_GEOMETRY_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKmath *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2008-12 Stanford University and the Authors. *
13  * Authors: Peter Eastman, Michael Sherman *
14  * Contributors: Ian Stavness, Andreas Scholz *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
31 #include "SimTKcommon.h"
36 
37 #include <cassert>
38 
39 namespace SimTK {
40 
44 SimTK_DEFINE_UNIQUE_INDEX_TYPE(ContactGeometryTypeId);
45 
46 class ContactGeometryImpl;
47 class OBBTreeNodeImpl;
48 class OBBTree;
49 class Plane;
50 
51 
52 
53 //==============================================================================
54 // CONTACT GEOMETRY
55 //==============================================================================
111 public:
112 class HalfSpace;
113 class Sphere;
114 class Ellipsoid;
115 class Torus;
116 class SmoothHeightMap;
117 class Cylinder;
118 class Brick;
119 class TriangleMesh;
120 
121 // TODO
122 class Cone;
123 
125 ContactGeometry() : impl(0) {}
127 ContactGeometry(const ContactGeometry& src);
129 ContactGeometry& operator=(const ContactGeometry& src);
133 ~ContactGeometry();
134 
136 DecorativeGeometry createDecorativeGeometry() const;
137 
148 Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
149 
164 Vec3 projectDownhillToNearestPoint(const Vec3& pointQ) const;
165 
230 bool trackSeparationFromLine(const Vec3& pointOnLine,
231  const UnitVec3& directionOfLine,
232  const Vec3& startingGuessForClosestPoint,
233  Vec3& newClosestPointOnSurface,
234  Vec3& closestPointOnLine,
235  Real& height) const;
236 
237 
238 
250 bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
251  Real& distance, UnitVec3& normal) const;
252 
258 void getBoundingSphere(Vec3& center, Real& radius) const;
259 
263 bool isSmooth() const;
264 
281 void calcCurvature(const Vec3& point, Vec2& curvature,
282  Rotation& orientation) const;
283 
294 const Function& getImplicitFunction() const;
295 
301 Real calcSurfaceValue(const Vec3& point) const;
302 
313 UnitVec3 calcSurfaceUnitNormal(const Vec3& point) const;
314 
320 Vec3 calcSurfaceGradient(const Vec3& point) const;
321 
327 Mat33 calcSurfaceHessian(const Vec3& point) const;
328 
357 Real calcGaussianCurvature(const Vec3& gradient,
358  const Mat33& Hessian) const;
359 
363 Real calcGaussianCurvature(const Vec3& point) const {
364  return calcGaussianCurvature(calcSurfaceGradient(point),
365  calcSurfaceHessian(point));
366 }
367 
376 Real calcSurfaceCurvatureInDirection(const Vec3& point, const UnitVec3& direction) const;
377 
386 void calcSurfacePrincipalCurvatures(const Vec3& point, Vec2& curvature,
387  Rotation& R_SP) const;
388 
391 bool isConvex() const;
392 
398 Vec3 calcSupportPoint(UnitVec3 direction) const;
399 
402 ContactGeometryTypeId getTypeId() const;
403 
454 static Vec2 evalParametricCurvature(const Vec3& P, const UnitVec3& nn,
455  const Vec3& dPdu, const Vec3& dPdv,
456  const Vec3& d2Pdu2, const Vec3& d2Pdv2,
457  const Vec3& d2Pdudv,
458  Transform& X_EP);
459 
533 static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
534  const UnitVec3& x2, const Vec2& k2,
535  Rotation& R_SP, Vec2& k);
536 
541 static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
542  const UnitVec3& x2, const Vec2& k2,
543  Vec2& k);
544 
545 
559 void initGeodesic(const Vec3& xP, const Vec3& xQ, const Vec3& xSP,
560  const GeodesicOptions& options, Geodesic& geod) const;
561 
562 
605 // XXX if xP and xQ are the exact end-points of prevGeod; then geod = prevGeod;
606 void continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod,
607  const GeodesicOptions& options, Geodesic& geod) const;
608 
635 void makeStraightLineGeodesic(const Vec3& xP, const Vec3& xQ,
636  const UnitVec3& defaultDirectionIfNeeded,
637  const GeodesicOptions& options, Geodesic& geod) const;
638 
639 
649 // XXX what to do if tP is not in the tangent plane at P -- project it?
650 void shootGeodesicInDirectionUntilLengthReached
651  (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength,
652  const GeodesicOptions& options, Geodesic& geod) const;
653 
667 void calcGeodesicReverseSensitivity
668  (Geodesic& geodesic,
669  const Vec2& initSensitivity = Vec2(0,1)) const; // j, jdot at end point
670 
671 
681 // XXX what to do if tP is not in the tangent plane at P -- project it?
682 // XXX what to do if we don't hit the plane
683 void shootGeodesicInDirectionUntilPlaneHit(const Vec3& xP, const UnitVec3& tP,
684  const Plane& terminatingPlane, const GeodesicOptions& options,
685  Geodesic& geod) const;
686 
687 
690 void calcGeodesic(const Vec3& xP, const Vec3& xQ,
691  const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
692 
695 void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ,
696  const Vec3& tPhint, Real lengthHint, Geodesic& geod) const;
697 
700 void calcGeodesicUsingOrthogonalMethod(const Vec3& xP, const Vec3& xQ,
701  Geodesic& geod) const
702 {
703  const Vec3 r_PQ = xQ - xP;
704  const Real lengthHint = r_PQ.norm();
705  const UnitVec3 n = calcSurfaceUnitNormal(xP);
706  // Project r_PQ into the tangent plane.
707  const Vec3 t_PQ = r_PQ - (~r_PQ*n)*n;
708  const Real tLength = t_PQ.norm();
709  const UnitVec3 tPhint =
710  tLength != 0 ? UnitVec3(t_PQ/tLength, true)
711  : n.perp(); // some arbitrary perpendicular to n
712  calcGeodesicUsingOrthogonalMethod(xP, xQ, Vec3(tPhint), lengthHint, geod);
713 }
714 
715 
723 Vec2 calcSplitGeodError(const Vec3& P, const Vec3& Q,
724  const UnitVec3& tP, const UnitVec3& tQ,
725  Geodesic* geod=0) const;
726 
727 
728 
739 // XXX what to do if tP is not in the tangent plane at P -- project it?
740 void shootGeodesicInDirectionUntilLengthReachedAnalytical
741  (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength,
742  const GeodesicOptions& options, Geodesic& geod) const;
743 
744 
755 // XXX what to do if tP is not in the tangent plane at P -- project it?
756 // XXX what to do if we don't hit the plane
757 void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3& xP, const UnitVec3& tP,
758  const Plane& terminatingPlane, const GeodesicOptions& options,
759  Geodesic& geod) const;
760 
761 
766 void calcGeodesicAnalytical(const Vec3& xP, const Vec3& xQ,
767  const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
768 
777 Vec2 calcSplitGeodErrorAnalytical(const Vec3& P, const Vec3& Q,
778  const UnitVec3& tP, const UnitVec3& tQ,
779  Geodesic* geod=0) const;
780 
791 const Plane& getPlane() const;
794 void setPlane(const Plane& plane) const;
796 const Geodesic& getGeodP() const;
798 const Geodesic& getGeodQ() const;
799 const int getNumGeodesicsShot() const;
800 void addVizReporter(ScheduledEventReporter* reporter) const;
805 explicit ContactGeometry(ContactGeometryImpl* impl);
806 bool isOwnerHandle() const;
807 bool isEmptyHandle() const;
808 bool hasImpl() const {return impl != 0;}
810 const ContactGeometryImpl& getImpl() const {assert(impl); return *impl;}
812 ContactGeometryImpl& updImpl() {assert(impl); return *impl; }
813 
814 protected:
815 ContactGeometryImpl* impl;
816 };
817 
818 
819 
820 //==============================================================================
821 // HALF SPACE
822 //==============================================================================
827 public:
833 HalfSpace();
834 
836 UnitVec3 getNormal() const;
837 
839 static bool isInstance(const ContactGeometry& geo)
840 { return geo.getTypeId()==classTypeId(); }
842 static const HalfSpace& getAs(const ContactGeometry& geo)
843 { assert(isInstance(geo)); return static_cast<const HalfSpace&>(geo); }
846 { assert(isInstance(geo)); return static_cast<HalfSpace&>(geo); }
847 
849 static ContactGeometryTypeId classTypeId();
850 
851 class Impl;
852 const Impl& getImpl() const;
853 Impl& updImpl();
854 };
855 
856 
857 
858 //==============================================================================
859 // CYLINDER
860 //==============================================================================
865 public:
866 explicit Cylinder(Real radius);
867 Real getRadius() const;
868 void setRadius(Real radius);
869 
871 static bool isInstance(const ContactGeometry& geo)
872 { return geo.getTypeId()==classTypeId(); }
874 static const Cylinder& getAs(const ContactGeometry& geo)
875 { assert(isInstance(geo)); return static_cast<const Cylinder&>(geo); }
878 { assert(isInstance(geo)); return static_cast<Cylinder&>(geo); }
879 
881 static ContactGeometryTypeId classTypeId();
882 
883 class Impl;
884 const Impl& getImpl() const;
885 Impl& updImpl();
886 };
887 
888 
889 
890 //==============================================================================
891 // SPHERE
892 //==============================================================================
896 public:
897 explicit Sphere(Real radius);
898 Real getRadius() const;
899 void setRadius(Real radius);
900 
902 static bool isInstance(const ContactGeometry& geo)
903 { return geo.getTypeId()==classTypeId(); }
905 static const Sphere& getAs(const ContactGeometry& geo)
906 { assert(isInstance(geo)); return static_cast<const Sphere&>(geo); }
909 { assert(isInstance(geo)); return static_cast<Sphere&>(geo); }
910 
912 static ContactGeometryTypeId classTypeId();
913 
914 class Impl;
915 const Impl& getImpl() const;
916 Impl& updImpl();
917 };
918 
919 
920 
921 //==============================================================================
922 // ELLIPSOID
923 //==============================================================================
946 public:
950 explicit Ellipsoid(const Vec3& radii);
953 const Vec3& getRadii() const;
959 void setRadii(const Vec3& radii);
960 
966 const Vec3& getCurvatures() const;
967 
980 UnitVec3 findUnitNormalAtPoint(const Vec3& P) const;
981 
989 Vec3 findPointWithThisUnitNormal(const UnitVec3& n) const;
990 
999 Vec3 findPointInSameDirection(const Vec3& Q) const;
1000 
1023 void findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const;
1024 
1030 void findParaboloidAtPointWithNormal(const Vec3& Q, const UnitVec3& n,
1031  Transform& X_EP, Vec2& k) const;
1032 
1034 static bool isInstance(const ContactGeometry& geo)
1035 { return geo.getTypeId()==classTypeId(); }
1037 static const Ellipsoid& getAs(const ContactGeometry& geo)
1038 { assert(isInstance(geo)); return static_cast<const Ellipsoid&>(geo); }
1041 { assert(isInstance(geo)); return static_cast<Ellipsoid&>(geo); }
1042 
1044 static ContactGeometryTypeId classTypeId();
1045 
1046 class Impl;
1047 const Impl& getImpl() const;
1048 Impl& updImpl();
1049 };
1050 
1051 
1052 
1053 //==============================================================================
1054 // SMOOTH HEIGHT MAP
1055 //==============================================================================
1068 public:
1072 explicit SmoothHeightMap(const BicubicSurface& surface);
1073 
1076 const BicubicSurface& getBicubicSurface() const;
1077 
1080 const OBBTree& getOBBTree() const;
1081 
1083 static bool isInstance(const ContactGeometry& geo)
1084 { return geo.getTypeId()==classTypeId(); }
1086 static const SmoothHeightMap& getAs(const ContactGeometry& geo)
1087 { assert(isInstance(geo)); return static_cast<const SmoothHeightMap&>(geo); }
1090 { assert(isInstance(geo)); return static_cast<SmoothHeightMap&>(geo); }
1091 
1093 static ContactGeometryTypeId classTypeId();
1094 
1095 class Impl;
1096 const Impl& getImpl() const;
1097 Impl& updImpl();
1098 };
1099 
1100 
1101 //==============================================================================
1102 // BRICK
1103 //==============================================================================
1107 public:
1110 explicit Brick(const Vec3& halfLengths);
1114 const Vec3& getHalfLengths() const;
1116 void setHalfLengths(const Vec3& halfLengths);
1117 
1119 const Geo::Box& getGeoBox() const;
1120 
1122 static bool isInstance(const ContactGeometry& geo)
1123 { return geo.getTypeId()==classTypeId(); }
1125 static const Brick& getAs(const ContactGeometry& geo)
1126 { assert(isInstance(geo)); return static_cast<const Brick&>(geo); }
1129 { assert(isInstance(geo)); return static_cast<Brick&>(geo); }
1130 
1132 static ContactGeometryTypeId classTypeId();
1133 
1134 class Impl;
1135 const Impl& getImpl() const;
1136 Impl& updImpl();
1137 };
1138 
1139 
1140 //==============================================================================
1141 // TRIANGLE MESH
1142 //==============================================================================
1164 : public ContactGeometry {
1165 public:
1166 class OBBTreeNode;
1177 TriangleMesh(const ArrayViewConst_<Vec3>& vertices, const ArrayViewConst_<int>& faceIndices, bool smooth=false);
1186 explicit TriangleMesh(const PolygonalMesh& mesh, bool smooth=false);
1188 int getNumEdges() const;
1190 int getNumFaces() const;
1192 int getNumVertices() const;
1196 const Vec3& getVertexPosition(int index) const;
1202 int getFaceEdge(int face, int edge) const;
1207 int getFaceVertex(int face, int vertex) const;
1212 int getEdgeFace(int edge, int face) const;
1217 int getEdgeVertex(int edge, int vertex) const;
1222 void findVertexEdges(int vertex, Array_<int>& edges) const;
1225 const UnitVec3& getFaceNormal(int face) const;
1228 Real getFaceArea(int face) const;
1234 Vec3 findPoint(int face, const Vec2& uv) const;
1239 Vec3 findCentroid(int face) const;
1244 UnitVec3 findNormalAtPoint(int face, const Vec2& uv) const;
1255 Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
1268 Vec3 findNearestPoint(const Vec3& position, bool& inside, int& face, Vec2& uv) const;
1269 
1279 Vec3 findNearestPointToFace(const Vec3& position, int face, Vec2& uv) const;
1280 
1281 
1293 bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const;
1307 bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, int& face, Vec2& uv) const;
1310 OBBTreeNode getOBBTreeNode() const;
1311 
1314 PolygonalMesh createPolygonalMesh() const;
1315 
1317 static bool isInstance(const ContactGeometry& geo)
1318 { return geo.getTypeId()==classTypeId(); }
1320 static const TriangleMesh& getAs(const ContactGeometry& geo)
1321 { assert(isInstance(geo)); return static_cast<const TriangleMesh&>(geo); }
1324 { assert(isInstance(geo)); return static_cast<TriangleMesh&>(geo); }
1325 
1327 static ContactGeometryTypeId classTypeId();
1328 
1329 class Impl;
1330 const Impl& getImpl() const;
1331 Impl& updImpl();
1332 };
1333 
1334 
1335 
1336 //==============================================================================
1337 // TRIANGLE MESH :: OBB TREE NODE
1338 //==============================================================================
1344 public:
1345 OBBTreeNode(const OBBTreeNodeImpl& impl);
1348 const OrientedBoundingBox& getBounds() const;
1350 bool isLeafNode() const;
1353 const OBBTreeNode getFirstChildNode() const;
1356 const OBBTreeNode getSecondChildNode() const;
1359 const Array_<int>& getTriangles() const;
1363 int getNumTriangles() const;
1364 
1365 private:
1366 const OBBTreeNodeImpl* impl;
1367 };
1368 
1369 //==============================================================================
1370 // TORUS
1371 //==============================================================================
1378 public:
1379 Torus(Real torusRadius, Real tubeRadius);
1380 Real getTorusRadius() const;
1381 void setTorusRadius(Real radius);
1382 Real getTubeRadius() const;
1383 void setTubeRadius(Real radius);
1384 
1386 static bool isInstance(const ContactGeometry& geo)
1387 { return geo.getTypeId()==classTypeId(); }
1389 static const Torus& getAs(const ContactGeometry& geo)
1390 { assert(isInstance(geo)); return static_cast<const Torus&>(geo); }
1393 { assert(isInstance(geo)); return static_cast<Torus&>(geo); }
1394 
1396 static ContactGeometryTypeId classTypeId();
1397 
1398 class Impl;
1399 const Impl& getImpl() const;
1400 Impl& updImpl();
1401 };
1402 
1403 
1404 
1405 
1406 //==============================================================================
1407 // GEODESIC EVALUATOR helper classes
1408 //==============================================================================
1409 
1410 
1414 class Plane {
1415 public:
1416  Plane() : m_normal(1,0,0), m_offset(0) { }
1417  Plane(const Vec3& normal, const Real& offset)
1418  : m_normal(normal), m_offset(offset) { }
1419 
1420  Real getDistance(const Vec3& pt) const {
1421  return ~m_normal*pt - m_offset;
1422  }
1423 
1424  Vec3 getNormal() const {
1425  return m_normal;
1426  }
1427 
1428  Real getOffset() const {
1429  return m_offset;
1430  }
1431 
1432 private:
1433  Vec3 m_normal;
1434  Real m_offset;
1435 }; // class Plane
1436 
1437 
1443 public:
1445  : TriggeredEventHandler(Stage::Position) { }
1446 
1447  explicit GeodHitPlaneEvent(const Plane& aplane)
1448  : TriggeredEventHandler(Stage::Position) {
1449  plane = aplane;
1450  }
1451 
1452  // event is triggered if distance of geodesic endpoint to plane is zero
1453  Real getValue(const State& state) const {
1454  if (!enabled) {
1455  return 1;
1456  }
1457  Vec3 endpt(&state.getQ()[0]);
1458  Real dist = plane.getDistance(endpt);
1459 // std::cout << "dist = " << dist << std::endl;
1460  return dist;
1461  }
1462 
1463  // This method is called whenever this event occurs.
1464  void handleEvent(State& state, Real accuracy, bool& shouldTerminate) const {
1465  if (!enabled) {
1466  return;
1467  }
1468 
1469  // This should be triggered when geodesic endpoint to plane is zero.
1470  Vec3 endpt;
1471  const Vector& q = state.getQ();
1472  endpt[0] = q[0]; endpt[1] = q[1]; endpt[2] = q[2];
1473  Real dist = plane.getDistance(endpt);
1474 
1475 // ASSERT(std::abs(dist) < 0.01 );
1476  shouldTerminate = true;
1477 // std::cout << "hit plane!" << std::endl;
1478  }
1479 
1480  void setPlane(const Plane& aplane) const {
1481  plane = aplane;
1482  }
1483 
1484  const Plane& getPlane() const {
1485  return plane;
1486  }
1487 
1488  const void setEnabled(bool enabledFlag) {
1489  enabled = enabledFlag;
1490  }
1491 
1492  const bool isEnabled() {
1493  return enabled;
1494  }
1495 
1496 private:
1497  mutable Plane plane;
1498  bool enabled;
1499 
1500 }; // class GeodHitPlaneEvent
1501 
1506 public:
1507  PathDecorator(const Vector& x, const Vec3& O, const Vec3& I, const Vec3& color) :
1508  m_x(x), m_O(O), m_I(I), m_color(color) { }
1509 
1510  virtual void generateDecorations(const State& state,
1511  Array_<DecorativeGeometry>& geometry) {
1512 // m_system.realize(state, Stage::Position);
1513 
1514  Vec3 P, Q;
1515  P[0] = m_x[0]; P[1] = m_x[1]; P[2] = m_x[2];
1516  Q[0] = m_x[3]; Q[1] = m_x[4]; Q[2] = m_x[5];
1517 
1518  geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_O));
1519  geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(P));
1520  geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(Q));
1521  geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_I));
1522 
1523  geometry.push_back(DecorativeLine(m_O,P)
1524  .setColor(m_color)
1525  .setLineThickness(2));
1526  geometry.push_back(DecorativeLine(Q,m_I)
1527  .setColor(m_color)
1528  .setLineThickness(2));
1529 
1530  }
1531 
1532 private:
1533  const Vector& m_x; // x = ~[P Q]
1534  const Vec3& m_O;
1535  const Vec3& m_I;
1536  const Vec3& m_color;
1537  Rotation R_plane;
1538  Vec3 offset;
1539 }; // class DecorationGenerator
1540 
1541 
1546 public:
1547  PlaneDecorator(const Plane& plane, const Vec3& color) :
1548  m_plane(plane), m_color(color) { }
1549 
1550  virtual void generateDecorations(const State& state,
1551  Array_<DecorativeGeometry>& geometry) {
1552 // m_system.realize(state, Stage::Position);
1553 
1554  // draw plane
1555  R_plane.setRotationFromOneAxis(UnitVec3(m_plane.getNormal()),
1557  offset = 0;
1558  offset[0] = m_plane.getOffset();
1559  geometry.push_back(
1560  DecorativeBrick(Vec3(Real(.01),1,1))
1561  .setTransform(Transform(R_plane, R_plane*offset))
1562  .setColor(m_color)
1563  .setOpacity(Real(.2)));
1564  }
1565 
1566 private:
1567  const Plane& m_plane;
1568  const Vec3& m_color;
1569  Rotation R_plane;
1570  Vec3 offset;
1571 }; // class DecorationGenerator
1572 
1573 
1574 } // namespace SimTK
1575 
1576 #endif // SimTK_SIMMATH_CONTACT_GEOMETRY_H_
This class will create a smooth surface that approximates a two-argument function F(X...
Definition: BicubicSurface.h:158
ScheduledEventReporter is a subclass of EventReporter for events that occur at a particular time that...
Definition: EventReporter.h:72
UnitVec< P, 1 > perp() const
Return a new unit vector perpendicular to this one but otherwise arbitrary.
Definition: UnitVec.h:182
Plane(const Vec3 &normal, const Real &offset)
Definition: ContactGeometry.h:1417
This defines a rectangular solid centered at the origin and aligned with the local frame axes...
Definition: DecorativeGeometry.h:424
SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex)
A simple plane class.
Definition: ContactGeometry.h:1414
UnitVec< Real, 1 > UnitVec3
Definition: UnitVec.h:41
static Ellipsoid & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable Ellipsoid.
Definition: ContactGeometry.h:1040
This class generates decoration for contact points and straight line path segments.
Definition: ContactGeometry.h:1505
static SmoothHeightMap & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable SmoothHeightMap.
Definition: ContactGeometry.h:1089
This is a unique integer type for quickly identifying specific types of contact geometry for fast loo...
const void setEnabled(bool enabledFlag)
Definition: ContactGeometry.h:1488
Plane()
Definition: ContactGeometry.h:1416
GeodHitPlaneEvent(const Plane &aplane)
Definition: ContactGeometry.h:1447
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is an Ellipsoid.
Definition: ContactGeometry.h:1034
This ContactGeometry subclass represents a torus centered at the origin with the axial direction alig...
Definition: ContactGeometry.h:1377
const bool isEnabled()
Definition: ContactGeometry.h:1492
Definition: CoordinateAxis.h:194
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:50
PathDecorator(const Vector &x, const Vec3 &O, const Vec3 &I, const Vec3 &color)
Definition: ContactGeometry.h:1507
This class stores a geodesic curve after it has been determined.
Definition: Geodesic.h:51
static const Brick & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const brick.
Definition: ContactGeometry.h:1125
static HalfSpace & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable halfspace.
Definition: ContactGeometry.h:845
This file defines the BicubicSurface class, and the BicubicFunction class that uses it to create a tw...
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a halfspace.
Definition: ContactGeometry.h:839
This ContactGeometry subclass represents a smooth surface fit through a set of sampled points using b...
Definition: ContactGeometry.h:1066
ContactGeometryImpl * impl
Internal use only.
Definition: ContactGeometry.h:815
A 3d rectangular box aligned with an unspecified frame F and centered at that frame&#39;s origin...
Definition: Geo.h:61
This class represents a rectangular box with arbitrary position and orientation.
Definition: OrientedBoundingBox.h:42
void handleEvent(State &state, Real accuracy, bool &shouldTerminate) const
This method is invoked to handle the event.
Definition: ContactGeometry.h:1464
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:593
virtual void generateDecorations(const State &state, Array_< DecorativeGeometry > &geometry)
This will be called every time a new State is about to be visualized.
Definition: ContactGeometry.h:1550
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a cylinder.
Definition: ContactGeometry.h:871
PlaneDecorator(const Plane &plane, const Vec3 &color)
Definition: ContactGeometry.h:1547
static const TriangleMesh & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const triangle mesh.
Definition: ContactGeometry.h:1320
static const Sphere & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const sphere.
Definition: ContactGeometry.h:905
ContactGeometryImpl & updImpl()
Internal use only.
Definition: ContactGeometry.h:812
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
void calcGeodesicUsingOrthogonalMethod(const Vec3 &xP, const Vec3 &xQ, Geodesic &geod) const
This signature makes a guess at the initial direction and length and then calls the other signature...
Definition: ContactGeometry.h:700
static const Cylinder & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const cylinder.
Definition: ContactGeometry.h:874
This class represents a node in the Oriented Bounding Box Tree for a TriangleMesh.
Definition: ContactGeometry.h:1343
Real calcGaussianCurvature(const Vec3 &point) const
This signature is for convenience; use the other one to save time if you already have the gradient an...
Definition: ContactGeometry.h:363
static Brick & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable brick.
Definition: ContactGeometry.h:1128
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i...
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
A DecorationGenerator is used to define geometry that may change over the course of a simulation...
Definition: DecorationGenerator.h:45
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a SmoothHeightMap.
Definition: ContactGeometry.h:1083
static Torus & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable torus.
Definition: ContactGeometry.h:1392
This class generates decoration for a plane.
Definition: ContactGeometry.h:1545
This class stores options for calculating geodesics.
Definition: Geodesic.h:311
static Sphere & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable sphere.
Definition: ContactGeometry.h:908
This ContactGeometry subclass represents an arbitrary shape described by a mesh of triangular faces...
Definition: ContactGeometry.h:1163
static const Ellipsoid & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const Ellipsoid.
Definition: ContactGeometry.h:1037
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a sphere.
Definition: ContactGeometry.h:902
static Cylinder & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable cylinder.
Definition: ContactGeometry.h:877
A ContactGeometry object describes the shape of all or part of the boundary of a solid object...
Definition: ContactGeometry.h:110
const Plane & getPlane() const
Definition: ContactGeometry.h:1484
static const SmoothHeightMap & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const SmoothHeightMap.
Definition: ContactGeometry.h:1086
Real getDistance(const Vec3 &pt) const
Definition: ContactGeometry.h:1420
This ContactGeometry subclass represents an object that occupies the entire half-space x>0...
Definition: ContactGeometry.h:826
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a triangle mesh.
Definition: ContactGeometry.h:1317
This class provides a description of a mesh made of polygonal faces (not limited to triangles)...
Definition: PolygonalMesh.h:70
gјiЌтkDdёMV wfлaIJуtюц ЩAАйЉt”џ‡1Лƒ  J†љŠcA nrш “S qо is†І3 уЏ ЇжЇ Ы –ТгVфЦKф Cž ‰д9Z €У ЉD Ўq FxnЙцТ n єT ЉY Н‡< щ['ЖjdЛ< K JvTMЯH"ЋˆяОsђ™Ыw>}љіo_Йљ…Зo”ЖЃ?Ї zџШЎ'z:mЄІуЧV$ќКРŠyЁnаgЭ–џiлИ”J{и ž”хЩїTaК*АдdКВŽE|lєzƒbXс@!^Рклѕ‘гOЌoi_г=ўінOП}&ŒкХВQUŠŠ­V…ЅWT­shЃ!ГљPЌ_‰ŸЊЕС7šЅDR›AVъfЎ‘вЪПЈЛЅbOждЙЩЋtЎ0оY!О|л'ђЉx'žўУГ§нЅК—:/ўќПќ V[ц,тЇо}-Bжž§Ъ/ПјиџіЌм‚;у:Žх;яОй;IюЫПЇо˜й[nK4Ћ#ІЁ-Б='ЈGf€\lЙчѕœb41лЉŸ> ѓйOчsчЂ7x f p§€ъZ„‘zB рI •рƒЮ gЊБЁ Њ ˆ n
Definition: SimmathUserGuide.doc:2262
This is the header file that every Simmath compilation unit should include first. ...
This is the client-side interface to an implementation-independent representation of "Decorations" su...
Definition: DecorativeGeometry.h:86
This defines a sphere centered at the origin.
Definition: DecorativeGeometry.h:365
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:48
This ContactGeometry subclass represents a sphere centered at the origin.
Definition: ContactGeometry.h:895
virtual void generateDecorations(const State &state, Array_< DecorativeGeometry > &geometry)
This will be called every time a new State is about to be visualized.
Definition: ContactGeometry.h:1510
TODO.
Definition: OBBTree.h:100
TriggeredEventHandler is a subclass of EventHandler for events that occur when some condition is sati...
Definition: EventHandler.h:109
Real getOffset() const
Definition: ContactGeometry.h:1428
static const Torus & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const torus.
Definition: ContactGeometry.h:1389
Vec3 getNormal() const
Definition: ContactGeometry.h:1424
const Vector & getQ(SubsystemIndex) const
Per-subsystem access to the global shared variables.
This ContactGeometry subclass represents a cylinder centered at the origin, with radius r in the x-y ...
Definition: ContactGeometry.h:864
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:608
bool hasImpl() const
Internal use only.
Definition: ContactGeometry.h:808
static TriangleMesh & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable triangle mesh.
Definition: ContactGeometry.h:1323
void setPlane(const Plane &aplane) const
Definition: ContactGeometry.h:1480
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a torus.
Definition: ContactGeometry.h:1386
ContactGeometry()
Base class default constructor creates an empty handle.
Definition: ContactGeometry.h:125
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a brick.
Definition: ContactGeometry.h:1122
const ContactGeometryImpl & getImpl() const
Internal use only.
Definition: ContactGeometry.h:810
static const HalfSpace & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const halfspace.
Definition: ContactGeometry.h:842
Vec< 3 > Vec3
This is the most common 3D vector type: a column of 3 Real values stored consecutively in memory (pac...
Definition: SmallMatrix.h:129
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
A line between two points.
Definition: DecorativeGeometry.h:304
Real getValue(const State &state) const
Get the value of the event trigger function for a State.
Definition: ContactGeometry.h:1453
void push_back(const T &value)
This method increases the size of the Array by one element at the end and initializes that element by...
Definition: Array.h:2359
This file defines the Geodesic class.
This ContactGeometry subclass represents an ellipsoid centered at the origin, with its principal axes...
Definition: ContactGeometry.h:945
A event handler to terminate integration when geodesic hits the plane.
Definition: ContactGeometry.h:1442
GeodHitPlaneEvent()
Definition: ContactGeometry.h:1444
however the hybrid system can request that if in which case “end of step” is treated like any other event External events are typically handled through the end of step event mechanism For the end of step function can look at the keyboard etc and respond accordingly a termination event occurs either when the integrator advances to a designated “final time” for the or when an event handler indicates that the simulation must be terminated for some other reason Event trigger transition details This section probably covers more than you really want to know about event triggers But if you run into some mysterious behavior regarding your event trigger functions at some point
Definition: SimmathUserGuide.doc:228
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
аЯ рЁБ с ўџ З Й ўџџџ Г Д Е Ж џџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџџьЅС € ј П ч bjbjcTcT кй Г У Л t џџ џџ џџ З Ÿ С K K K D џџџџ    € ‹2 ”  ZЦ j J a n u a r A b s t r a c t W e d e s c r i b e t h e g o a l s a n d d e s i g n d e c i s i o n b e h i n d S i m m a t r i x
Definition: Simmatrix.doc:5
Transform_< Real > Transform
Definition: Transform.h:44
ContactGeometryTypeId getTypeId() const
ContactTrackerSubsystem uses this id for fast identification of specific surface shapes.
Vec< 2 > Vec2
This is the most common 2D vector type: a column of 2 Real values stored consecutively in memory (pac...
Definition: SmallMatrix.h:126
const Vec3 Black
RGB=( 0, 0, 0)
This ContactGeometry subclass represents a rectangular solid centered at the origin.
Definition: ContactGeometry.h:1106