Simbody 3.7
Loading...
Searching...
No Matches
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
39namespace SimTK {
40
45
46class ContactGeometryImpl;
47class OBBTreeNodeImpl;
48class OBBTree;
49class Plane;
50
51
52
53//==============================================================================
54// CONTACT GEOMETRY
55//==============================================================================
111public:
112class HalfSpace;
113class Sphere;
114class Ellipsoid;
115class Torus;
116class SmoothHeightMap;
117class Cylinder;
118class Brick;
119class TriangleMesh;
120
121// TODO
122class Cone;
123
125ContactGeometry() : impl(0) {}
134
137
148Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
149
165
230bool 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
250bool intersectsRay(const Vec3& origin, const UnitVec3& direction,
251 Real& distance, UnitVec3& normal) const;
252
258void getBoundingSphere(Vec3& center, Real& radius) const;
259
263bool isSmooth() const;
264
281void calcCurvature(const Vec3& point, Vec2& curvature,
282 Rotation& orientation) const;
283
295
301Real calcSurfaceValue(const Vec3& point) const;
302
314
320Vec3 calcSurfaceGradient(const Vec3& point) const;
321
327Mat33 calcSurfaceHessian(const Vec3& point) const;
328
358 const Mat33& Hessian) const;
359
363Real calcGaussianCurvature(const Vec3& point) const {
364 return calcGaussianCurvature(calcSurfaceGradient(point),
365 calcSurfaceHessian(point));
366}
367
376Real calcSurfaceCurvatureInDirection(const Vec3& point, const UnitVec3& direction) const;
377
386void calcSurfacePrincipalCurvatures(const Vec3& point, Vec2& curvature,
387 Rotation& R_SP) const;
388
391bool isConvex() const;
392
399
403
454static 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
533static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
534 const UnitVec3& x2, const Vec2& k2,
535 Rotation& R_SP, Vec2& k);
536
541static void combineParaboloids(const Rotation& R_SP1, const Vec2& k1,
542 const UnitVec3& x2, const Vec2& k2,
543 Vec2& k);
544
545
559void 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;
606void continueGeodesic(const Vec3& xP, const Vec3& xQ, const Geodesic& prevGeod,
607 const GeodesicOptions& options, Geodesic& geod) const;
608
635void 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
690void calcGeodesic(const Vec3& xP, const Vec3& xQ,
691 const Vec3& tPhint, const Vec3& tQhint, Geodesic& geod) const;
692
696 const Vec3& tPhint, Real lengthHint, Geodesic& geod) const;
697
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
723Vec2 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
766void 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
791const Plane& getPlane() const;
794void setPlane(const Plane& plane) const;
796const Geodesic& getGeodP() const;
798const Geodesic& getGeodQ() const;
799const int getNumGeodesicsShot() const;
805explicit ContactGeometry(ContactGeometryImpl* impl);
806bool isOwnerHandle() const;
807bool isEmptyHandle() const;
808bool hasImpl() const {return impl != 0;}
810const ContactGeometryImpl& getImpl() const {assert(impl); return *impl;}
812ContactGeometryImpl& updImpl() {assert(impl); return *impl; }
813
814protected:
815ContactGeometryImpl* impl;
816};
817
818
819
820//==============================================================================
821// HALF SPACE
822//==============================================================================
827public:
834
837
839static bool isInstance(const ContactGeometry& geo)
840{ return geo.getTypeId()==classTypeId(); }
842static 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
851class Impl;
852const Impl& getImpl() const;
853Impl& updImpl();
854};
855
856
857
858//==============================================================================
859// CYLINDER
860//==============================================================================
865public:
866explicit Cylinder(Real radius);
868void setRadius(Real radius);
869
871static bool isInstance(const ContactGeometry& geo)
872{ return geo.getTypeId()==classTypeId(); }
874static 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
883class Impl;
884const Impl& getImpl() const;
885Impl& updImpl();
886};
887
888
889
890//==============================================================================
891// SPHERE
892//==============================================================================
896public:
897explicit Sphere(Real radius);
899void setRadius(Real radius);
900
902static bool isInstance(const ContactGeometry& geo)
903{ return geo.getTypeId()==classTypeId(); }
905static 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
914class Impl;
915const Impl& getImpl() const;
916Impl& updImpl();
917};
918
919
920
921//==============================================================================
922// ELLIPSOID
923//==============================================================================
946public:
950explicit Ellipsoid(const Vec3& radii);
953const Vec3& getRadii() const;
959void setRadii(const Vec3& radii);
960
966const Vec3& getCurvatures() const;
967
981
990
1000
1023void findParaboloidAtPoint(const Vec3& Q, Transform& X_EP, Vec2& k) const;
1024
1031 Transform& X_EP, Vec2& k) const;
1032
1034static bool isInstance(const ContactGeometry& geo)
1035{ return geo.getTypeId()==classTypeId(); }
1037static 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
1046class Impl;
1047const Impl& getImpl() const;
1048Impl& updImpl();
1049};
1050
1051
1052
1053//==============================================================================
1054// SMOOTH HEIGHT MAP
1055//==============================================================================
1068public:
1072explicit SmoothHeightMap(const BicubicSurface& surface);
1073
1077
1080const OBBTree& getOBBTree() const;
1081
1083static bool isInstance(const ContactGeometry& geo)
1084{ return geo.getTypeId()==classTypeId(); }
1086static 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
1095class Impl;
1096const Impl& getImpl() const;
1097Impl& updImpl();
1098};
1099
1100
1101//==============================================================================
1102// BRICK
1103//==============================================================================
1107public:
1110explicit Brick(const Vec3& halfLengths);
1114const Vec3& getHalfLengths() const;
1116void setHalfLengths(const Vec3& halfLengths);
1117
1119const Geo::Box& getGeoBox() const;
1120
1122static bool isInstance(const ContactGeometry& geo)
1123{ return geo.getTypeId()==classTypeId(); }
1125static 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
1134class Impl;
1135const Impl& getImpl() const;
1136Impl& updImpl();
1137};
1138
1139
1140//==============================================================================
1141// TRIANGLE MESH
1142//==============================================================================
1164: public ContactGeometry {
1165public:
1166class OBBTreeNode;
1177TriangleMesh(const ArrayViewConst_<Vec3>& vertices, const ArrayViewConst_<int>& faceIndices, bool smooth=false);
1186explicit TriangleMesh(const PolygonalMesh& mesh, bool smooth=false);
1188int getNumEdges() const;
1190int getNumFaces() const;
1192int getNumVertices() const;
1196const Vec3& getVertexPosition(int index) const;
1202int getFaceEdge(int face, int edge) const;
1207int getFaceVertex(int face, int vertex) const;
1212int getEdgeFace(int edge, int face) const;
1217int getEdgeVertex(int edge, int vertex) const;
1222void findVertexEdges(int vertex, Array_<int>& edges) const;
1225const UnitVec3& getFaceNormal(int face) const;
1228Real getFaceArea(int face) const;
1234Vec3 findPoint(int face, const Vec2& uv) const;
1239Vec3 findCentroid(int face) const;
1244UnitVec3 findNormalAtPoint(int face, const Vec2& uv) const;
1255Vec3 findNearestPoint(const Vec3& position, bool& inside, UnitVec3& normal) const;
1268Vec3 findNearestPoint(const Vec3& position, bool& inside, int& face, Vec2& uv) const;
1269
1279Vec3 findNearestPointToFace(const Vec3& position, int face, Vec2& uv) const;
1280
1281
1293bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, UnitVec3& normal) const;
1307bool intersectsRay(const Vec3& origin, const UnitVec3& direction, Real& distance, int& face, Vec2& uv) const;
1311
1315
1317static bool isInstance(const ContactGeometry& geo)
1318{ return geo.getTypeId()==classTypeId(); }
1320static 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
1329class Impl;
1330const Impl& getImpl() const;
1331Impl& updImpl();
1332};
1333
1334
1335
1336//==============================================================================
1337// TRIANGLE MESH :: OBB TREE NODE
1338//==============================================================================
1344public:
1345OBBTreeNode(const OBBTreeNodeImpl& impl);
1350bool isLeafNode() const;
1364
1365private:
1366const OBBTreeNodeImpl* impl;
1367};
1368
1369//==============================================================================
1370// TORUS
1371//==============================================================================
1378public:
1379Torus(Real torusRadius, Real tubeRadius);
1383void setTubeRadius(Real radius);
1384
1386static bool isInstance(const ContactGeometry& geo)
1387{ return geo.getTypeId()==classTypeId(); }
1389static 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
1398class Impl;
1399const Impl& getImpl() const;
1400Impl& updImpl();
1401};
1402
1403
1404
1405
1406//==============================================================================
1407// GEODESIC EVALUATOR helper classes
1408//==============================================================================
1409
1410
1414class Plane {
1415public:
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
1432private:
1433 Vec3 m_normal;
1434 Real m_offset;
1435}; // class Plane
1436
1437
1443public:
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
1492private:
1493 mutable Plane plane;
1494 bool enabled;
1495
1496}; // class GeodHitPlaneEvent
1497
1502public:
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
1528private:
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
1542public:
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
1562private:
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.
#define SimTK_DEFINE_UNIQUE_INDEX_TYPE(NAME)
Use this macro to define a unique "Index" type which is just a type-safe non-negative int,...
Definition SimTKcommon/include/SimTKcommon/internal/common.h:426
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
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition Array.h:1520
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 Geo::Box & getGeoBox() const
Get the Geo::Box object used to represent this Brick.
void setHalfLengths(const Vec3 &halfLengths)
Change the shape or size of this brick by setting its half-dimensions.
const Impl & getImpl() const
Internal use only.
Brick(const Vec3 &halfLengths)
Create a brick-shaped contact shape of the given half-dimensions, expressed in the brick's local fram...
Impl & updImpl()
Internal use only.
static ContactGeometryTypeId classTypeId()
Obtain the unique id for Brick contact geometry.
static const Brick & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const brick.
Definition ContactGeometry.h:1125
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a brick.
Definition ContactGeometry.h:1122
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
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 ContactGeometryTypeId classTypeId()
Obtain the unique id for Cylinder contact geometry.
static const Cylinder & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const cylinder.
Definition ContactGeometry.h:874
Impl & updImpl()
Internal use only.
const Impl & getImpl() const
Internal use only.
static Cylinder & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable cylinder.
Definition ContactGeometry.h:877
This ContactGeometry subclass represents an ellipsoid centered at the origin, with its principal axes...
Definition ContactGeometry.h:945
Impl & updImpl()
Internal use only.
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 ...
const Vec3 & getCurvatures() const
For efficiency we precalculate the principal curvatures whenever the ellipsoid radii are set; this av...
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 Impl & getImpl() const
Internal use only.
static ContactGeometryTypeId classTypeId()
Obtain the unique id for Ellipsoid contact geometry.
static Ellipsoid & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable Ellipsoid.
Definition ContactGeometry.h:1040
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,...
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...
static const HalfSpace & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const halfspace.
Definition ContactGeometry.h:842
static ContactGeometryTypeId classTypeId()
Obtain the unique id for HalfSpace contact geometry.
const Impl & getImpl() const
Internal use only.
static HalfSpace & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable halfspace.
Definition ContactGeometry.h:845
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a halfspace.
Definition ContactGeometry.h:839
Impl & updImpl()
Internal use only.
UnitVec3 getNormal() const
Return the HalfSpace outward normal in its own frame as a unit vector.
This ContactGeometry subclass represents a smooth surface fit through a set of sampled points using b...
Definition ContactGeometry.h:1067
static SmoothHeightMap & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable SmoothHeightMap.
Definition ContactGeometry.h:1089
Impl & updImpl()
Internal use only.
static ContactGeometryTypeId classTypeId()
Obtain the unique id for SmoothHeightMap contact geometry.
const OBBTree & getOBBTree() const
(Advanced) Return a reference to the oriented bounding box tree for this surface.
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a SmoothHeightMap.
Definition ContactGeometry.h:1083
const BicubicSurface & getBicubicSurface() const
Return a reference to the BicubicSurface object being used by this SmoothHeightMap.
const Impl & getImpl() const
Internal use only.
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
static const Sphere & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const sphere.
Definition ContactGeometry.h:905
static Sphere & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable sphere.
Definition ContactGeometry.h:908
Impl & updImpl()
Internal use only.
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a sphere.
Definition ContactGeometry.h:902
static ContactGeometryTypeId classTypeId()
Obtain the unique id for Sphere contact geometry.
const Impl & getImpl() const
Internal use only.
This ContactGeometry subclass represents a torus centered at the origin with the axial direction alig...
Definition ContactGeometry.h:1377
void setTubeRadius(Real radius)
static Torus & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable torus.
Definition ContactGeometry.h:1392
Torus(Real torusRadius, Real tubeRadius)
static bool isInstance(const ContactGeometry &geo)
Return true if the supplied ContactGeometry object is a torus.
Definition ContactGeometry.h:1386
const Impl & getImpl() const
Internal use only.
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.
Impl & updImpl()
Internal use only.
void setTorusRadius(Real radius)
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 OBBTreeNode getSecondChildNode() const
Get the second child node.
int getNumTriangles() const
Get the number of triangles inside this 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.
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.
static const TriangleMesh & getAs(const ContactGeometry &geo)
Cast the supplied ContactGeometry object to a const triangle mesh.
Definition ContactGeometry.h:1320
Real getFaceArea(int face) const
Get the area of a face.
static ContactGeometryTypeId classTypeId()
Obtain the unique id for TriangleMesh contact geometry.
const Impl & getImpl() const
Internal use only.
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.
Impl & updImpl()
Internal use only.
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.
const Vec3 & getVertexPosition(int index) const
Get the position of a vertex in the mesh.
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.
const UnitVec3 & getFaceNormal(int face) const
Get the normal vector for a face.
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 TriangleMesh & updAs(ContactGeometry &geo)
Cast the supplied ContactGeometry object to a writable triangle mesh.
Definition ContactGeometry.h:1323
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
ContactGeometryImpl & updImpl()
Internal use only.
Definition ContactGeometry.h:812
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...
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...
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 ...
const Plane & getPlane() const
Get the plane associated with the geodesic hit plane event handler
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...
const ContactGeometryImpl & getImpl() const
Internal use only.
Definition ContactGeometry.h:810
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 ...
const Function & getImplicitFunction() const
Our smooth surfaces define a function f(P)=0 that provides an implicit representation of the surface.
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,...
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,...
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,...
const Geodesic & getGeodP() const
Get the geodesic for access by visualizer.
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.
const Geodesic & getGeodQ() const
Get the geodesic for access by visualizer.
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.
ContactGeometry & operator=(const ContactGeometry &src)
Copy assignment makes a deep copy.
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.
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
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
const Plane & getPlane() const
Definition ContactGeometry.h:1480
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< 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:44
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:46