Simbody  3.6
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 
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) {}
134 
137 
148 Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
149 
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 
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 
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 
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?
651  (const Vec3& xP, const UnitVec3& tP, const Real& terminatingLength,
652  const GeodesicOptions& options, Geodesic& geod) const;
653 
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
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?
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
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 
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;
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:
834 
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 
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 
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 
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 
981 
990 
1000 
1023 void findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const;
1024 
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 
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 
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 
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 
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;
1311 
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 
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);
1350 bool isLeafNode() 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);
1381 void setTorusRadius(Real radius);
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 
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 override {
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 override {
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  shouldTerminate = true;
1474  }
1475 
1476  void setPlane(const Plane& aplane) const {
1477  plane = aplane;
1478  }
1479 
1480  const Plane& getPlane() const {
1481  return plane;
1482  }
1483 
1484  const void setEnabled(bool enabledFlag) {
1485  enabled = enabledFlag;
1486  }
1487 
1488  const bool isEnabled() {
1489  return enabled;
1490  }
1491 
1492 private:
1493  mutable Plane plane;
1494  bool enabled;
1495 
1496 }; // class GeodHitPlaneEvent
1497 
1502 public:
1503  PathDecorator(const Vector& x, const Vec3& O, const Vec3& I, const Vec3& color) :
1504  m_x(x), m_O(O), m_I(I), m_color(color) { }
1505 
1506  virtual void generateDecorations(const State& state,
1507  Array_<DecorativeGeometry>& geometry) override {
1508 // m_system.realize(state, Stage::Position);
1509 
1510  Vec3 P, Q;
1511  P[0] = m_x[0]; P[1] = m_x[1]; P[2] = m_x[2];
1512  Q[0] = m_x[3]; Q[1] = m_x[4]; Q[2] = m_x[5];
1513 
1514  geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_O));
1515  geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(P));
1516  geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(Q));
1517  geometry.push_back(DecorativeSphere(Real(.05)).setColor(Black).setTransform(m_I));
1518 
1519  geometry.push_back(DecorativeLine(m_O,P)
1520  .setColor(m_color)
1521  .setLineThickness(2));
1522  geometry.push_back(DecorativeLine(Q,m_I)
1523  .setColor(m_color)
1524  .setLineThickness(2));
1525 
1526  }
1527 
1528 private:
1529  const Vector& m_x; // x = ~[P Q]
1530  const Vec3& m_O;
1531  const Vec3& m_I;
1532  const Vec3& m_color;
1533  Rotation R_plane;
1534  Vec3 offset;
1535 }; // class DecorationGenerator
1536 
1537 
1542 public:
1543  PlaneDecorator(const Plane& plane, const Vec3& color) :
1544  m_plane(plane), m_color(color) { }
1545 
1546  virtual void generateDecorations(const State& state,
1547  Array_<DecorativeGeometry>& geometry) override {
1548 // m_system.realize(state, Stage::Position);
1549 
1550  // draw plane
1551  R_plane.setRotationFromOneAxis(UnitVec3(m_plane.getNormal()),
1553  offset = 0;
1554  offset[0] = m_plane.getOffset();
1555  geometry.push_back(
1556  DecorativeBrick(Vec3(Real(.01),1,1))
1557  .setTransform(Transform(R_plane, R_plane*offset))
1558  .setColor(m_color)
1559  .setOpacity(Real(.2)));
1560  }
1561 
1562 private:
1563  const Plane& m_plane;
1564  const Vec3& m_color;
1565  Rotation R_plane;
1566  Vec3 offset;
1567 }; // class DecorationGenerator
1568 
1569 
1570 } // namespace SimTK
1571 
1572 #endif // SimTK_SIMMATH_CONTACT_GEOMETRY_H_
This file defines the BicubicSurface class, and the BicubicFunction class that uses it to create a tw...
This file defines the Geodesic class.
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:324
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:2399
This class will create a smooth surface that approximates a two-argument function F(X,...
Definition: BicubicSurface.h:158
This is a unique integer type for quickly identifying specific types of contact geometry for fast loo...
This ContactGeometry subclass represents a rectangular solid centered at the origin.
Definition: ContactGeometry.h:1106
const Vec3 & getHalfLengths() const
Get the half-dimensions of this Brick, expressed in its own frame.
static Brick & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable brick.
Definition: ContactGeometry.h:1128
void setHalfLengths(const Vec3 &halfLengths)
Change the shape or size of this brick by setting its half-dimensions.
Brick(const Vec3 &halfLengths)
Create a brick-shaped contact shape of the given half-dimensions, expressed in the brick's local fram...
static ContactGeometryTypeId classTypeId()
Obtain the unique id for Brick contact geometry.
Impl & updImpl()
Internal use only.
const Impl & getImpl() const
Internal use only.
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a brick.
Definition: ContactGeometry.h:1122
static const Brick & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const brick.
Definition: ContactGeometry.h:1125
const Geo::Box & getGeoBox() const
Get the Geo::Box object used to represent this Brick.
This ContactGeometry subclass represents a cylinder centered at the origin, with radius r in the x-y ...
Definition: ContactGeometry.h:864
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a cylinder.
Definition: ContactGeometry.h:871
static Cylinder & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable cylinder.
Definition: ContactGeometry.h:877
static const Cylinder & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const cylinder.
Definition: ContactGeometry.h:874
static ContactGeometryTypeId classTypeId()
Obtain the unique id for Cylinder contact geometry.
Impl & updImpl()
Internal use only.
const Impl & getImpl() const
Internal use only.
This ContactGeometry subclass represents an ellipsoid centered at the origin, with its principal axes...
Definition: ContactGeometry.h:945
Ellipsoid(const Vec3 &radii)
Construct an Ellipsoid given its three principal half-axis dimensions a,b,c (all positive) along the ...
void findParaboloidAtPoint(const Vec3 &Q, Transform &X_EP, Vec2 &k) const
Given a point Q on the surface of the ellipsoid, find the approximating paraboloid at Q in a frame P ...
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 an Ellipsoid.
Definition: ContactGeometry.h:1034
const Vec3 & getCurvatures() const
For efficiency we precalculate the principal curvatures whenever the ellipsoid radii are set; this av...
Impl & updImpl()
Internal use only.
static ContactGeometryTypeId classTypeId()
Obtain the unique id for Ellipsoid contact geometry.
const Impl & getImpl() const
Internal use only.
void setRadii(const Vec3 &radii)
Set the three half-axis dimensions a,b,c (all positive) used to define this ellipsoid,...
Vec3 findPointInSameDirection(const Vec3 &Q) const
Given a direction d defined by the vector Q-O for an arbitrary point in space Q=(x,...
static Ellipsoid & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable Ellipsoid.
Definition: ContactGeometry.h:1040
void findParaboloidAtPointWithNormal(const Vec3 &Q, const UnitVec3 &n, Transform &X_EP, Vec2 &k) const
If you already have both a point and the unit normal at that point, this will save about 40 flops by ...
Vec3 findPointWithThisUnitNormal(const UnitVec3 &n) const
Given a unit direction n, find the unique point P on the ellipsoid surface at which the outward-facin...
const Vec3 & getRadii() const
Obtain the three half-axis dimensions a,b,c used to define this ellipsoid.
UnitVec3 findUnitNormalAtPoint(const Vec3 &P) const
Given a point P =(x,y,z) on the ellipsoid surface, return the unique unit outward normal to the ellip...
This ContactGeometry subclass represents an object that occupies the entire half-space x>0.
Definition: ContactGeometry.h:826
HalfSpace()
Create a HalfSpace for contact, with surface passing through the origin and outward normal -XAxis (in...
Impl & updImpl()
Internal use only.
static ContactGeometryTypeId classTypeId()
Obtain the unique id for HalfSpace contact geometry.
const Impl & getImpl() const
Internal use only.
static const HalfSpace & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const halfspace.
Definition: ContactGeometry.h:842
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a halfspace.
Definition: ContactGeometry.h:839
UnitVec3 getNormal() const
Return the HalfSpace outward normal in its own frame as a unit vector.
static HalfSpace & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable halfspace.
Definition: ContactGeometry.h:845
This ContactGeometry subclass represents a smooth surface fit through a set of sampled points using b...
Definition: ContactGeometry.h:1067
const Impl & getImpl() const
Internal use only.
const OBBTree & getOBBTree() const
(Advanced) Return a reference to the oriented bounding box tree for this surface.
static ContactGeometryTypeId classTypeId()
Obtain the unique id for SmoothHeightMap contact geometry.
const BicubicSurface & getBicubicSurface() const
Return a reference to the BicubicSurface object being used by this SmoothHeightMap.
Impl & updImpl()
Internal use only.
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a SmoothHeightMap.
Definition: ContactGeometry.h:1083
static SmoothHeightMap & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable SmoothHeightMap.
Definition: ContactGeometry.h:1089
static const SmoothHeightMap & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const SmoothHeightMap.
Definition: ContactGeometry.h:1086
SmoothHeightMap(const BicubicSurface &surface)
Create a SmoothHeightMap surface using an already-existing BicubicSurface.
This ContactGeometry subclass represents a sphere centered at the origin.
Definition: ContactGeometry.h:895
Impl & updImpl()
Internal use only.
static Sphere & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable sphere.
Definition: ContactGeometry.h:908
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a sphere.
Definition: ContactGeometry.h:902
const Impl & getImpl() const
Internal use only.
static const Sphere & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const sphere.
Definition: ContactGeometry.h:905
static ContactGeometryTypeId classTypeId()
Obtain the unique id for Sphere contact geometry.
This ContactGeometry subclass represents a torus centered at the origin with the axial direction alig...
Definition: ContactGeometry.h:1377
void setTubeRadius(Real radius)
const Impl & getImpl() const
Internal use only.
Torus(Real torusRadius, Real tubeRadius)
Impl & updImpl()
Internal use only.
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a torus.
Definition: ContactGeometry.h:1386
static const Torus & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const torus.
Definition: ContactGeometry.h:1389
static ContactGeometryTypeId classTypeId()
Obtain the unique id for Torus contact geometry.
void setTorusRadius(Real radius)
static Torus & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable torus.
Definition: ContactGeometry.h:1392
This class represents a node in the Oriented Bounding Box Tree for a TriangleMesh.
Definition: ContactGeometry.h:1343
const OBBTreeNode getFirstChildNode() const
Get the first child node.
bool isLeafNode() const
Get whether this is a leaf node.
const OrientedBoundingBox & getBounds() const
Get the OrientedBoundingBox which encloses all triangles in this node or its children.
const Array_< int > & getTriangles() const
Get the indices of all triangles contained in this node.
const OBBTreeNode getSecondChildNode() const
Get the second child node.
int getNumTriangles() const
Get the number of triangles inside this node.
This ContactGeometry subclass represents an arbitrary shape described by a mesh of triangular faces.
Definition: ContactGeometry.h:1164
Vec3 findNearestPoint(const Vec3 &position, bool &inside, int &face, Vec2 &uv) const
Given a point, find the nearest point on the surface of this object.
int getFaceEdge(int face, int edge) const
Get the index of one of the edges of a face.
int getEdgeVertex(int edge, int vertex) const
Get the index of one of the vertices shared by an edge.
Impl & updImpl()
Internal use only.
Real getFaceArea(int face) const
Get the area of a face.
static ContactGeometryTypeId classTypeId()
Obtain the unique id for TriangleMesh contact geometry.
const UnitVec3 & getFaceNormal(int face) const
Get the normal vector for a face.
const Impl & getImpl() const
Internal use only.
static TriangleMesh & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable triangle mesh.
Definition: ContactGeometry.h:1323
const Vec3 & getVertexPosition(int index) const
Get the position of a vertex in the mesh.
int getNumEdges() const
Get the number of edges in the mesh.
OBBTreeNode getOBBTreeNode() const
Get the OBBTreeNode which forms the root of this mesh's Oriented Bounding Box Tree.
Vec3 findCentroid(int face) const
Calculate the location of a face's centroid, that is, the point uv=(1/3,1/3) which is the average of ...
Vec3 findNearestPointToFace(const Vec3 &position, int face, Vec2 &uv) const
Given a point and a face of this object, find the point of the face that is nearest the given point.
UnitVec3 findNormalAtPoint(int face, const Vec2 &uv) const
Calculate the normal vector at a point on the surface.
Vec3 findPoint(int face, const Vec2 &uv) const
Calculate the location of a point on the surface, in the local frame of the TriangleMesh.
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a triangle mesh.
Definition: ContactGeometry.h:1317
bool intersectsRay(const Vec3 &origin, const UnitVec3 &direction, Real &distance, UnitVec3 &normal) const
Determine whether this mesh intersects a ray, and if so, find the intersection point.
TriangleMesh(const ArrayViewConst_< Vec3 > &vertices, const ArrayViewConst_< int > &faceIndices, bool smooth=false)
Create a TriangleMesh.
Vec3 findNearestPoint(const Vec3 &position, bool &inside, UnitVec3 &normal) const
Given a point, find the nearest point on the surface of this object.
int getEdgeFace(int edge, int face) const
Get the index of one of the faces shared by an edge.
TriangleMesh(const PolygonalMesh &mesh, bool smooth=false)
Create a TriangleMesh based on a PolygonalMesh object.
int getNumVertices() const
Get the number of vertices in the mesh.
int getNumFaces() const
Get the number of faces in the mesh.
int getFaceVertex(int face, int vertex) const
Get the index of one of the vertices of a face.
static const TriangleMesh & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const triangle mesh.
Definition: ContactGeometry.h:1320
bool intersectsRay(const Vec3 &origin, const UnitVec3 &direction, Real &distance, int &face, Vec2 &uv) const
Determine whether this mesh intersects a ray, and if so, find what face it hit.
PolygonalMesh createPolygonalMesh() const
Generate a PolygonalMesh from this TriangleMesh; useful mostly for debugging because you can create a...
void findVertexEdges(int vertex, Array_< int > &edges) const
Find all edges that intersect a vertex.
A ContactGeometry object describes the shape of all or part of the boundary of a solid object,...
Definition: ContactGeometry.h:110
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
ContactGeometryImpl * impl
Internal use only.
Definition: ContactGeometry.h:815
bool isConvex() const
Returns true if this surface is known to be convex.
void calcGeodesicUsingOrthogonalMethod(const Vec3 &xP, const Vec3 &xQ, const Vec3 &tPhint, Real lengthHint, Geodesic &geod) const
Utility method to find geodesic between P and Q using the orthogonal method, with initial direction t...
UnitVec3 calcSurfaceUnitNormal(const Vec3 &point) const
Calculate the implicit surface outward facing unit normal at the given point.
Vec2 calcSplitGeodErrorAnalytical(const Vec3 &P, const Vec3 &Q, const UnitVec3 &tP, const UnitVec3 &tQ, Geodesic *geod=0) const
Utility method to analytically calculate the "geodesic error" between one geodesic shot from P in the...
static void combineParaboloids(const Rotation &R_SP1, const Vec2 &k1, const UnitVec3 &x2, const Vec2 &k2, Vec2 &k)
This is a much faster version of combineParaboloids() for when you just need the curvatures of the di...
const int getNumGeodesicsShot() const
Get the plane associated with the geodesic hit plane event handler
void getBoundingSphere(Vec3 &center, Real &radius) const
Get a bounding sphere which completely encloses this object.
void calcCurvature(const Vec3 &point, Vec2 &curvature, Rotation &orientation) const
Compute the principal curvatures and their directions, and the surface normal, at a given point on a ...
DecorativeGeometry createDecorativeGeometry() const
Generate a DecorativeGeometry that matches the shape of this ContactGeometry.
void setPlane(const Plane &plane) const
Set the plane associated with the geodesic hit plane event handler
void calcGeodesicReverseSensitivity(Geodesic &geodesic, const Vec2 &initSensitivity=Vec2(0, 1)) const
Given an already-calculated geodesic on this surface connecting points P and Q, fill in the sensitivi...
const Plane & getPlane() const
Get the plane associated with the geodesic hit plane event handler
Vec2 calcSplitGeodError(const Vec3 &P, const Vec3 &Q, const UnitVec3 &tP, const UnitVec3 &tQ, Geodesic *geod=0) const
Utility method to calculate the "geodesic error" between one geodesic shot from P in the direction tP...
const Function & getImplicitFunction() const
Our smooth surfaces define a function f(P)=0 that provides an implicit representation of the surface.
bool hasImpl() const
Internal use only.
Definition: ContactGeometry.h:808
void calcGeodesicAnalytical(const Vec3 &xP, const Vec3 &xQ, const Vec3 &tPhint, const Vec3 &tQhint, Geodesic &geod) const
Utility method to analytically find geodesic between P and Q with initial shooting directions tPhint ...
Vec3 calcSurfaceGradient(const Vec3 &point) const
Calculate the gradient of the implicit surface function, at a given point.
~ContactGeometry()
Base class destructor deletes the implementation object. Note that this is not virtual; handles shoul...
Vec3 findNearestPoint(const Vec3 &position, bool &inside, UnitVec3 &normal) const
Given a point, find the nearest point on the surface of this object.
bool isSmooth() const
Returns true if this is a smooth surface, meaning that it can provide meaningful curvature informatio...
bool trackSeparationFromLine(const Vec3 &pointOnLine, const UnitVec3 &directionOfLine, const Vec3 &startingGuessForClosestPoint, Vec3 &newClosestPointOnSurface, Vec3 &closestPointOnLine, Real &height) const
Track the closest point between this implicit surface and a given line, or the point of deepest penet...
bool intersectsRay(const Vec3 &origin, const UnitVec3 &direction, Real &distance, UnitVec3 &normal) const
Determine whether this object intersects a ray, and if so, find the intersection point.
Mat33 calcSurfaceHessian(const Vec3 &point) const
Calculate the hessian of the implicit surface function, at a given point.
void shootGeodesicInDirectionUntilLengthReachedAnalytical(const Vec3 &xP, const UnitVec3 &tP, const Real &terminatingLength, const GeodesicOptions &options, Geodesic &geod) const
Analytically compute a geodesic curve starting at the given point, starting in the given direction,...
Vec3 calcSupportPoint(UnitVec3 direction) const
Given a direction expressed in the surface's frame S, return the point P on the surface that is the f...
void calcGeodesic(const Vec3 &xP, const Vec3 &xQ, const Vec3 &tPhint, const Vec3 &tQhint, Geodesic &geod) const
Utility method to find geodesic between P and Q using split geodesic method with initial shooting dir...
Real calcGaussianCurvature(const Vec3 &gradient, const Mat33 &Hessian) const
For an implicit surface, return the Gaussian curvature at the point p whose implicit surface function...
static void combineParaboloids(const Rotation &R_SP1, const Vec2 &k1, const UnitVec3 &x2, const Vec2 &k2, Rotation &R_SP, Vec2 &k)
This utility method is useful for characterizing the relative geometry of two locally-smooth surfaces...
void continueGeodesic(const Vec3 &xP, const Vec3 &xQ, const Geodesic &prevGeod, const GeodesicOptions &options, Geodesic &geod) const
Given the current positions of two points P and Q moving on this surface, and the previous geodesic c...
ContactGeometry(const ContactGeometry &src)
Copy constructor makes a deep copy.
ContactGeometry()
Base class default constructor creates an empty handle.
Definition: ContactGeometry.h:125
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
void makeStraightLineGeodesic(const Vec3 &xP, const Vec3 &xQ, const UnitVec3 &defaultDirectionIfNeeded, const GeodesicOptions &options, Geodesic &geod) const
Produce a straight-line approximation to the (presumably short) geodesic between two points on this i...
bool isEmptyHandle() const
Internal use only.
Vec3 projectDownhillToNearestPoint(const Vec3 &pointQ) const
Given a query point Q, find the nearest point P on the surface of this object, looking only down the ...
static Vec2 evalParametricCurvature(const Vec3 &P, const UnitVec3 &nn, const Vec3 &dPdu, const Vec3 &dPdv, const Vec3 &d2Pdu2, const Vec3 &d2Pdv2, const Vec3 &d2Pdudv, Transform &X_EP)
Calculate surface curvature at a point using differential geometry as suggested by Harris 2006,...
void shootGeodesicInDirectionUntilLengthReached(const Vec3 &xP, const UnitVec3 &tP, const Real &terminatingLength, const GeodesicOptions &options, Geodesic &geod) const
Compute a geodesic curve starting at the given point, starting in the given direction,...
const ContactGeometryImpl & getImpl() const
Internal use only.
Definition: ContactGeometry.h:810
void shootGeodesicInDirectionUntilPlaneHitAnalytical(const Vec3 &xP, const UnitVec3 &tP, const Plane &terminatingPlane, const GeodesicOptions &options, Geodesic &geod) const
Analytically compute a geodesic curve starting at the given point, starting in the given direction,...
const Geodesic & getGeodP() const
Get the geodesic for access by visualizer.
bool isOwnerHandle() const
Internal use only.
void calcSurfacePrincipalCurvatures(const Vec3 &point, Vec2 &curvature, Rotation &R_SP) const
For an implicit surface at a given point p, return the principal curvatures and principal curvature d...
void shootGeodesicInDirectionUntilPlaneHit(const Vec3 &xP, const UnitVec3 &tP, const Plane &terminatingPlane, const GeodesicOptions &options, Geodesic &geod) const
Compute a geodesic curve starting at the given point, starting in the given direction,...
Real calcSurfaceCurvatureInDirection(const Vec3 &point, const UnitVec3 &direction) const
For an implicit surface, return the curvature k of the surface at a given point p in a given directio...
ContactGeometry(ContactGeometryImpl *impl)
Internal use only.
Real calcSurfaceValue(const Vec3 &point) const
Calculate the value of the implicit surface function, at a given point.
void addVizReporter(ScheduledEventReporter *reporter) const
Get the plane associated with the geodesic hit plane event handler
ContactGeometryTypeId getTypeId() const
ContactTrackerSubsystem uses this id for fast identification of specific surface shapes.
const Geodesic & getGeodQ() const
Get the geodesic for access by visualizer.
ContactGeometryImpl & updImpl()
Internal use only.
Definition: ContactGeometry.h:812
void initGeodesic(const Vec3 &xP, const Vec3 &xQ, const Vec3 &xSP, const GeodesicOptions &options, Geodesic &geod) const
Given two points, find a geodesic curve connecting them.
ContactGeometry & operator=(const ContactGeometry &src)
Copy assignment makes a deep copy.
Definition: CoordinateAxis.h:194
A DecorationGenerator is used to define geometry that may change over the course of a simulation.
Definition: DecorationGenerator.h:45
This defines a rectangular solid centered at the origin and aligned with the local frame axes.
Definition: DecorativeGeometry.h:426
This is the client-side interface to an implementation-independent representation of "Decorations" su...
Definition: DecorativeGeometry.h:86
A line between two points.
Definition: DecorativeGeometry.h:306
This defines a sphere centered at the origin.
Definition: DecorativeGeometry.h:367
A 3d rectangular box aligned with an unspecified frame F and centered at that frame's origin.
Definition: Geo_Box.h:48
A event handler to terminate integration when geodesic hits the plane.
Definition: ContactGeometry.h:1442
GeodHitPlaneEvent()
Definition: ContactGeometry.h:1444
const bool isEnabled()
Definition: ContactGeometry.h:1488
void setPlane(const Plane &aplane) const
Definition: ContactGeometry.h:1476
const Plane & getPlane() const
Definition: ContactGeometry.h:1480
GeodHitPlaneEvent(const Plane &aplane)
Definition: ContactGeometry.h:1447
const void setEnabled(bool enabledFlag)
Definition: ContactGeometry.h:1484
Real getValue(const State &state) const override
Get the value of the event trigger function for a State.
Definition: ContactGeometry.h:1453
void handleEvent(State &state, Real accuracy, bool &shouldTerminate) const override
This method is invoked to handle the event.
Definition: ContactGeometry.h:1464
This class stores options for calculating geodesics.
Definition: Geodesic.h:311
This class stores a geodesic curve after it has been determined.
Definition: Geodesic.h:51
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: Mat.h:97
TODO.
Definition: OBBTree.h:100
This class represents a rectangular box with arbitrary position and orientation.
Definition: OrientedBoundingBox.h:42
This class generates decoration for contact points and straight line path segments.
Definition: ContactGeometry.h:1501
virtual void generateDecorations(const State &state, Array_< DecorativeGeometry > &geometry) override
This will be called every time a new State is about to be visualized.
Definition: ContactGeometry.h:1506
PathDecorator(const Vector &x, const Vec3 &O, const Vec3 &I, const Vec3 &color)
Definition: ContactGeometry.h:1503
This class generates decoration for a plane.
Definition: ContactGeometry.h:1541
virtual void generateDecorations(const State &state, Array_< DecorativeGeometry > &geometry) override
This will be called every time a new State is about to be visualized.
Definition: ContactGeometry.h:1546
PlaneDecorator(const Plane &plane, const Vec3 &color)
Definition: ContactGeometry.h:1543
A simple plane class.
Definition: ContactGeometry.h:1414
Real getOffset() const
Definition: ContactGeometry.h:1428
Vec3 getNormal() const
Definition: ContactGeometry.h:1424
Real getDistance(const Vec3 &pt) const
Definition: ContactGeometry.h:1420
Plane(const Vec3 &normal, const Real &offset)
Definition: ContactGeometry.h:1417
Plane()
Definition: ContactGeometry.h:1416
This class provides a description of a mesh made of polygonal faces (not limited to triangles).
Definition: PolygonalMesh.h:71
Rotation_ & setRotationFromOneAxis(const UnitVec3P &uvec, CoordinateAxis axis)
Calculate R_AB by knowing one of B's unit vectors expressed in A.
ScheduledEventReporter is a subclass of EventReporter for events that occur at a particular time that...
Definition: EventReporter.h:72
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:66
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
const Vector & getQ(SubsystemIndex) const
Per-subsystem access to the global shared variables.
TriggeredEventHandler is a subclass of EventHandler for events that occur when some condition is sati...
Definition: EventHandler.h:109
UnitVec< P, 1 > perp() const
Return a new unit vector perpendicular to this one but otherwise arbitrary.
Definition: UnitVec.h:181
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:610
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
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
const Complex I
We only need one complex constant, i = sqrt(-1). For the rest just multiply the real constant by i,...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
const Vec3 Black
RGB=( 0, 0, 0)
UnitVec< Real, 1 > UnitVec3
Definition: UnitVec.h:41
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:606
Transform_< Real > Transform
Definition: Transform.h:44
SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex)