1 #ifndef SimTK_SIMMATH_CONTACT_GEOMETRY_H_ 2 #define SimTK_SIMMATH_CONTACT_GEOMETRY_H_ 46 class ContactGeometryImpl;
47 class OBBTreeNodeImpl;
148 Vec3 findNearestPoint(
const Vec3& position,
bool& inside,
UnitVec3& normal)
const;
164 Vec3 projectDownhillToNearestPoint(
const Vec3& pointQ)
const;
230 bool trackSeparationFromLine(
const Vec3& pointOnLine,
232 const Vec3& startingGuessForClosestPoint,
233 Vec3& newClosestPointOnSurface,
234 Vec3& closestPointOnLine,
250 bool intersectsRay(
const Vec3& origin,
const UnitVec3& direction,
258 void getBoundingSphere(
Vec3& center,
Real& radius)
const;
263 bool isSmooth()
const;
281 void calcCurvature(
const Vec3& point,
Vec2& curvature,
294 const Function& getImplicitFunction()
const;
301 Real calcSurfaceValue(
const Vec3& point)
const;
313 UnitVec3 calcSurfaceUnitNormal(
const Vec3& point)
const;
320 Vec3 calcSurfaceGradient(
const Vec3& point)
const;
327 Mat33 calcSurfaceHessian(
const Vec3& point)
const;
357 Real calcGaussianCurvature(
const Vec3& gradient,
358 const Mat33& Hessian)
const;
364 return calcGaussianCurvature(calcSurfaceGradient(point),
365 calcSurfaceHessian(point));
376 Real calcSurfaceCurvatureInDirection(
const Vec3& point,
const UnitVec3& direction)
const;
386 void calcSurfacePrincipalCurvatures(
const Vec3& point,
Vec2& curvature,
391 bool isConvex()
const;
456 const Vec3& d2Pdu2,
const Vec3& d2Pdv2,
533 static void combineParaboloids(
const Rotation& R_SP1,
const Vec2& k1,
541 static void combineParaboloids(
const Rotation& R_SP1,
const Vec2& k1,
559 void initGeodesic(
const Vec3& xP,
const Vec3& xQ,
const Vec3& xSP,
606 void continueGeodesic(
const Vec3& xP,
const Vec3& xQ,
const Geodesic& prevGeod,
635 void makeStraightLineGeodesic(
const Vec3& xP,
const Vec3& xQ,
636 const UnitVec3& defaultDirectionIfNeeded,
650 void shootGeodesicInDirectionUntilLengthReached
667 void calcGeodesicReverseSensitivity
669 const Vec2& initSensitivity =
Vec2(0,1))
const;
683 void shootGeodesicInDirectionUntilPlaneHit(
const Vec3& xP,
const UnitVec3& tP,
690 void calcGeodesic(
const Vec3& xP,
const Vec3& xQ,
695 void calcGeodesicUsingOrthogonalMethod(
const Vec3& xP,
const Vec3& xQ,
703 const Vec3 r_PQ = xQ - xP;
704 const Real lengthHint = r_PQ.
norm();
705 const UnitVec3 n = calcSurfaceUnitNormal(xP);
707 const Vec3 t_PQ = r_PQ - (~r_PQ*n)*n;
710 tLength != 0 ?
UnitVec3(t_PQ/tLength,
true)
712 calcGeodesicUsingOrthogonalMethod(xP, xQ,
Vec3(tPhint), lengthHint, geod);
740 void shootGeodesicInDirectionUntilLengthReachedAnalytical
757 void shootGeodesicInDirectionUntilPlaneHitAnalytical(
const Vec3& xP,
const UnitVec3& tP,
766 void calcGeodesicAnalytical(
const Vec3& xP,
const Vec3& xQ,
777 Vec2 calcSplitGeodErrorAnalytical(
const Vec3& P,
const Vec3& Q,
791 const Plane& getPlane()
const;
794 void setPlane(
const Plane& plane)
const;
799 const int getNumGeodesicsShot()
const;
806 bool isOwnerHandle()
const;
807 bool isEmptyHandle()
const;
810 const ContactGeometryImpl&
getImpl()
const {assert(impl);
return *impl;}
812 ContactGeometryImpl&
updImpl() {assert(impl);
return *impl; }
840 {
return geo.
getTypeId()==classTypeId(); }
843 { assert(isInstance(geo));
return static_cast<const HalfSpace&
>(geo); }
846 { assert(isInstance(geo));
return static_cast<HalfSpace&
>(geo); }
852 const Impl& getImpl()
const;
867 Real getRadius()
const;
868 void setRadius(
Real radius);
872 {
return geo.
getTypeId()==classTypeId(); }
875 { assert(isInstance(geo));
return static_cast<const Cylinder&
>(geo); }
878 { assert(isInstance(geo));
return static_cast<Cylinder&
>(geo); }
884 const Impl& getImpl()
const;
898 Real getRadius()
const;
899 void setRadius(
Real radius);
903 {
return geo.
getTypeId()==classTypeId(); }
906 { assert(isInstance(geo));
return static_cast<const Sphere&
>(geo); }
909 { assert(isInstance(geo));
return static_cast<Sphere&
>(geo); }
915 const Impl& getImpl()
const;
953 const Vec3& getRadii()
const;
959 void setRadii(
const Vec3& radii);
966 const Vec3& getCurvatures()
const;
989 Vec3 findPointWithThisUnitNormal(
const UnitVec3& n)
const;
999 Vec3 findPointInSameDirection(
const Vec3& Q)
const;
1030 void findParaboloidAtPointWithNormal(
const Vec3& Q,
const UnitVec3& n,
1035 {
return geo.
getTypeId()==classTypeId(); }
1038 { assert(isInstance(geo));
return static_cast<const Ellipsoid&
>(geo); }
1041 { assert(isInstance(geo));
return static_cast<Ellipsoid&
>(geo); }
1047 const Impl& getImpl()
const;
1080 const OBBTree& getOBBTree()
const;
1084 {
return geo.
getTypeId()==classTypeId(); }
1087 { assert(isInstance(geo));
return static_cast<const SmoothHeightMap&
>(geo); }
1090 { assert(isInstance(geo));
return static_cast<SmoothHeightMap&
>(geo); }
1096 const Impl& getImpl()
const;
1110 explicit Brick(
const Vec3& halfLengths);
1114 const Vec3& getHalfLengths()
const;
1116 void setHalfLengths(
const Vec3& halfLengths);
1123 {
return geo.
getTypeId()==classTypeId(); }
1126 { assert(isInstance(geo));
return static_cast<const Brick&
>(geo); }
1129 { assert(isInstance(geo));
return static_cast<Brick&
>(geo); }
1135 const Impl& getImpl()
const;
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;
1279 Vec3 findNearestPointToFace(
const Vec3& position,
int face,
Vec2& uv)
const;
1307 bool intersectsRay(
const Vec3& origin,
const UnitVec3& direction,
Real& distance,
int& face,
Vec2& uv)
const;
1318 {
return geo.
getTypeId()==classTypeId(); }
1321 { assert(isInstance(geo));
return static_cast<const TriangleMesh&
>(geo); }
1324 { assert(isInstance(geo));
return static_cast<TriangleMesh&
>(geo); }
1330 const Impl& getImpl()
const;
1350 bool isLeafNode()
const;
1363 int getNumTriangles()
const;
1366 const OBBTreeNodeImpl* impl;
1380 Real getTorusRadius()
const;
1381 void setTorusRadius(
Real radius);
1382 Real getTubeRadius()
const;
1383 void setTubeRadius(
Real radius);
1387 {
return geo.
getTypeId()==classTypeId(); }
1390 { assert(isInstance(geo));
return static_cast<const Torus&
>(geo); }
1393 { assert(isInstance(geo));
return static_cast<Torus&
>(geo); }
1399 const Impl& getImpl()
const;
1416 Plane() : m_normal(1,0,0), m_offset(0) { }
1418 : m_normal(normal), m_offset(offset) { }
1421 return ~m_normal*pt - m_offset;
1458 Real dist = plane.getDistance(endpt);
1472 endpt[0] = q[0]; endpt[1] = q[1]; endpt[2] = q[2];
1473 shouldTerminate =
true;
1485 enabled = enabledFlag;
1493 mutable Plane plane;
1504 m_x(x), m_O(O), m_I(I), m_color(color) { }
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];
1521 .setLineThickness(2));
1524 .setLineThickness(2));
1532 const Vec3& m_color;
1544 m_plane(plane), m_color(color) { }
1551 R_plane.setRotationFromOneAxis(
UnitVec3(m_plane.getNormal()),
1554 offset[0] = m_plane.getOffset();
1557 .setTransform(
Transform(R_plane, R_plane*offset))
1559 .setOpacity(
Real(.2)));
1563 const Plane& m_plane;
1564 const Vec3& m_color;
1572 #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
const Vector & getQ(SubsystemIndex) const
Per-subsystem access to the global shared variables.
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:426
SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex)
A simple plane class.
Definition: ContactGeometry.h:1414
UnitVec< Real, 1 > UnitVec3
Definition: UnitVec.h:41
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
const void setEnabled(bool enabledFlag)
Definition: ContactGeometry.h:1484
Real getOffset() const
Definition: ContactGeometry.h:1428
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
const bool isEnabled()
Definition: ContactGeometry.h:1488
Definition: CoordinateAxis.h:194
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:66
PathDecorator(const Vector &x, const Vec3 &O, const Vec3 &I, const Vec3 &color)
Definition: ContactGeometry.h:1503
This class stores a geodesic curve after it has been determined.
Definition: Geodesic.h:51
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
This file defines the BicubicSurface class, and the BicubicFunction class that uses it to create a tw...
A 3d rectangular box aligned with an unspecified frame F and centered at that frame's origin...
Definition: Geo.h:61
This class represents a rectangular box with arbitrary position and orientation.
Definition: OrientedBoundingBox.h:42
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
PlaneDecorator(const Plane &plane, const Vec3 &color)
Definition: ContactGeometry.h:1543
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:610
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
void handleEvent(State &state, Real accuracy, bool &shouldTerminate) const override
This method is invoked to handle the event.
Definition: ContactGeometry.h:1464
UnitVec< P, 1 > perp() const
Return a new unit vector perpendicular to this one but otherwise arbitrary.
Definition: UnitVec.h:181
Real getValue(const State &state) const override
Get the value of the event trigger function for a State.
Definition: ContactGeometry.h:1453
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
Real getDistance(const Vec3 &pt) const
Definition: ContactGeometry.h:1420
This class generates decoration for a plane.
Definition: ContactGeometry.h:1541
This class stores options for calculating geodesics.
Definition: Geodesic.h:311
void setPlane(const Plane &aplane) const
Definition: ContactGeometry.h:1476
This class provides a description of a mesh made of polygonal faces (not limited to triangles)...
Definition: PolygonalMesh.h:70
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:367
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:51
const Plane & getPlane() const
Definition: ContactGeometry.h:1480
TODO.
Definition: OBBTree.h:100
TriggeredEventHandler is a subclass of EventHandler for events that occur when some condition is sati...
Definition: EventHandler.h:109
Vec3 getNormal() const
Definition: ContactGeometry.h:1424
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:620
A line between two points.
Definition: DecorativeGeometry.h:306
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 file defines the Geodesic class.
A event handler to terminate integration when geodesic hits the plane.
Definition: ContactGeometry.h:1442
GeodHitPlaneEvent()
Definition: ContactGeometry.h:1444
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
Transform_< Real > Transform
Definition: Transform.h:44
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)