Simbody  3.5
ContactTracker.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
00002 #define SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                        Simbody(tm): SimTKmath                              *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2010-14 Stanford University and the Authors.        *
00013  * Authors: Michael Sherman, Peter Eastman                                    *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00027 #include "SimTKcommon.h"
00028 #include "simmath/internal/common.h"
00029 #include "simmath/internal/Contact.h"
00030 
00031 namespace SimTK {
00032 
00033 //==============================================================================
00034 //                              CONTACT TRACKER
00035 //==============================================================================
00062 class SimTK_SIMMATH_EXPORT ContactTracker {
00063 public:
00064 class HalfSpaceSphere;
00065 class HalfSpaceEllipsoid;
00066 class HalfSpaceBrick;
00067 class HalfSpaceTriangleMesh;
00068 class HalfSpaceConvexImplicit;
00069 class SphereSphere;
00070 class SphereTriangleMesh;
00071 class TriangleMeshTriangleMesh;
00072 class ConvexImplicitPair;
00073 class GeneralImplicitPair;
00074 
00076 ContactTracker(ContactGeometryTypeId typeOfSurface1,
00077                ContactGeometryTypeId typeOfSurface2)
00078 :   m_surfaceTypes(typeOfSurface1, typeOfSurface2)
00079 {
00080 }
00081 
00084 const std::pair<ContactGeometryTypeId,ContactGeometryTypeId>&
00085 getContactGeometryTypeIds() const {return m_surfaceTypes;}
00086 
00087 virtual ~ContactTracker() {}
00088 
00094 virtual bool trackContact
00095    (const Contact&         priorStatus,
00096     const Transform& X_GS1, 
00097     const ContactGeometry& surface1,
00098     const Transform& X_GS2, 
00099     const ContactGeometry& surface2,
00100     Real                   cutoff,
00101     Contact&               currentStatus) const = 0;
00102 
00129 static bool refineImplicitPair
00130    (const ContactGeometry& shapeA, Vec3& pointP_A,    // in/out
00131     const ContactGeometry& shapeB, Vec3& pointQ_B,    // in/out
00132     const Transform& X_AB, Real accuracyRequested,
00133     Real& accuracyAchieved, int& numIterations);
00134 
00136 static Vec6 findImplicitPairError
00137    (const ContactGeometry& shapeA, const Vec3& pointP,
00138     const ContactGeometry& shapeB, const Vec3& pointQ,
00139     const Transform& X_AB);
00140 
00145 static Mat66 calcImplicitPairJacobian
00146    (const ContactGeometry& shapeA, const Vec3& pointP,
00147     const ContactGeometry& shapeB, const Vec3& pointQ,
00148     const Transform& X_AB, const Vec6& err0);
00149 
00160 static bool estimateConvexImplicitPairContactUsingMPR
00161    (const ContactGeometry& shapeA, const ContactGeometry& shapeB, 
00162     const Transform& X_AB,
00163     Vec3& pointP_A, Vec3& pointQ_B, UnitVec3& dirInA,
00164     int& numIterations);
00165 
00166 
00167 //--------------------------------------------------------------------------
00168                                 private:
00169 // This tracker should be called only for surfaces of these two types,
00170 // in this order.
00171 std::pair<ContactGeometryTypeId,ContactGeometryTypeId> m_surfaceTypes;
00172 };
00173 
00174 
00175 
00176 //==============================================================================
00177 //                     HALFSPACE-SPHERE CONTACT TRACKER
00178 //==============================================================================
00181 class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceSphere 
00182 :   public ContactTracker {
00183 public:
00184 HalfSpaceSphere() 
00185 :   ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
00186                    ContactGeometry::Sphere::classTypeId()) {}
00187 
00188 bool trackContact
00189    (const Contact&         priorStatus,
00190     const Transform& X_GS1, 
00191     const ContactGeometry& surface1,
00192     const Transform& X_GS2, 
00193     const ContactGeometry& surface2,
00194     Real                   cutoff,
00195     Contact&               currentStatus) const OVERRIDE_11;
00196 };
00197 
00198 
00199 
00200 //==============================================================================
00201 //                     HALFSPACE-ELLIPSOID CONTACT TRACKER
00202 //==============================================================================
00205 class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceEllipsoid 
00206 :   public ContactTracker {
00207 public:
00208 HalfSpaceEllipsoid() 
00209 :   ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
00210                    ContactGeometry::Ellipsoid::classTypeId()) {}
00211 
00212 bool trackContact
00213    (const Contact&         priorStatus,
00214     const Transform& X_GS1, 
00215     const ContactGeometry& surface1,
00216     const Transform& X_GS2, 
00217     const ContactGeometry& surface2,
00218     Real                   cutoff,
00219     Contact&               currentStatus) const OVERRIDE_11;
00220 };
00221 
00222 
00223 
00224 //==============================================================================
00225 //                     HALFSPACE-BRICK CONTACT TRACKER
00226 //==============================================================================
00229 class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceBrick 
00230 :   public ContactTracker {
00231 public:
00232 HalfSpaceBrick() 
00233 :   ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
00234                    ContactGeometry::Brick::classTypeId()) {}
00235 
00236 bool trackContact
00237    (const Contact&         priorStatus,
00238     const Transform& X_GS1, 
00239     const ContactGeometry& surface1,
00240     const Transform& X_GS2, 
00241     const ContactGeometry& surface2,
00242     Real                   cutoff,
00243     Contact&               currentStatus) const OVERRIDE_11;
00244 };
00245 
00246 
00247 
00248 //==============================================================================
00249 //                       SPHERE-SPHERE CONTACT TRACKER
00250 //==============================================================================
00253 class SimTK_SIMMATH_EXPORT ContactTracker::SphereSphere 
00254 :   public ContactTracker {
00255 public:
00256 SphereSphere() 
00257 :   ContactTracker(ContactGeometry::Sphere::classTypeId(),
00258                    ContactGeometry::Sphere::classTypeId()) {}
00259 
00260 bool trackContact
00261    (const Contact&         priorStatus,
00262     const Transform& X_GS1, 
00263     const ContactGeometry& surface1,
00264     const Transform& X_GS2, 
00265     const ContactGeometry& surface2,
00266     Real                   cutoff,
00267     Contact&               currentStatus) const OVERRIDE_11;
00268 };
00269 
00270 
00271 
00272 //==============================================================================
00273 //                 HALFSPACE-TRIANGLE MESH CONTACT TRACKER
00274 //==============================================================================
00277 class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceTriangleMesh
00278 :   public ContactTracker {
00279 public:
00280 HalfSpaceTriangleMesh() 
00281 :   ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
00282                    ContactGeometry::TriangleMesh::classTypeId()) {}
00283 
00284 bool trackContact
00285    (const Contact&         priorStatus,
00286     const Transform& X_GS1, 
00287     const ContactGeometry& surface1,    // the half space
00288     const Transform& X_GS2, 
00289     const ContactGeometry& surface2,    // the mesh
00290     Real                   cutoff,
00291     Contact&               currentStatus) const OVERRIDE_11;
00292 
00293 private:
00294 void processBox(const ContactGeometry::TriangleMesh&              mesh, 
00295                 const ContactGeometry::TriangleMesh::OBBTreeNode& node, 
00296                 const Transform& X_HM, const UnitVec3& hsNormal_M, 
00297                 Real hsFaceHeight_M, std::set<int>& insideFaces) const;
00298 void addAllTriangles(const ContactGeometry::TriangleMesh::OBBTreeNode& node, 
00299                      std::set<int>& insideFaces) const; 
00300 };
00301 
00302 
00303 
00304 //==============================================================================
00305 //                 SPHERE - TRIANGLE MESH CONTACT TRACKER
00306 //==============================================================================
00309 class SimTK_SIMMATH_EXPORT ContactTracker::SphereTriangleMesh
00310 :   public ContactTracker {
00311 public:
00312 SphereTriangleMesh() 
00313 :   ContactTracker(ContactGeometry::Sphere::classTypeId(),
00314                    ContactGeometry::TriangleMesh::classTypeId()) {}
00315 
00316 bool trackContact
00317    (const Contact&         priorStatus,
00318     const Transform& X_GS1, 
00319     const ContactGeometry& surface1,    // the sphere
00320     const Transform& X_GS2, 
00321     const ContactGeometry& surface2,    // the mesh
00322     Real                   cutoff,
00323     Contact&               currentStatus) const OVERRIDE_11;
00324 
00325 private:
00326 void processBox
00327    (const ContactGeometry::TriangleMesh&              mesh, 
00328     const ContactGeometry::TriangleMesh::OBBTreeNode& node, 
00329     const Vec3& center_M, Real radius2,   
00330     std::set<int>& insideFaces) const ;
00331 };
00332 
00333 
00334 
00335 //==============================================================================
00336 //             TRIANGLE MESH - TRIANGLE MESH CONTACT TRACKER
00337 //==============================================================================
00340 class SimTK_SIMMATH_EXPORT ContactTracker::TriangleMeshTriangleMesh
00341 :   public ContactTracker {
00342 public:
00343 TriangleMeshTriangleMesh() 
00344 :   ContactTracker(ContactGeometry::TriangleMesh::classTypeId(),
00345                    ContactGeometry::TriangleMesh::classTypeId()) {}
00346 
00347 bool trackContact
00348    (const Contact&         priorStatus,
00349     const Transform& X_GS1, 
00350     const ContactGeometry& surface1,    // mesh1
00351     const Transform& X_GS2, 
00352     const ContactGeometry& surface2,    // mesh2
00353     Real                   cutoff,
00354     Contact&               currentStatus) const OVERRIDE_11;
00355 
00356 private:
00357 void findIntersectingFaces
00358    (const ContactGeometry::TriangleMesh&                mesh1, 
00359     const ContactGeometry::TriangleMesh&                mesh2,
00360     const ContactGeometry::TriangleMesh::OBBTreeNode&   node1, 
00361     const ContactGeometry::TriangleMesh::OBBTreeNode&   node2, 
00362     const OrientedBoundingBox&                          node2Bounds_M1,
00363     const Transform&                                    X_M1M2, 
00364     std::set<int>&                                      insideFaces1, 
00365     std::set<int>&                                      insideFaces2) const; 
00366 
00367 void findBuriedFaces
00368    (const ContactGeometry::TriangleMesh&    mesh,
00369     const ContactGeometry::TriangleMesh&    otherMesh,
00370     const Transform&                        X_OM, 
00371     std::set<int>&                          insideFaces) const;
00372 
00373 void tagFaces(const ContactGeometry::TriangleMesh&   mesh, 
00374               Array_<int>&                           faceType,
00375               std::set<int>&                         triangles, 
00376               int                                    index,
00377               int                                    depth) const;
00378 };
00379 
00380 
00381 //==============================================================================
00382 //                 HALFSPACE-CONVEX IMPLICIT CONTACT TRACKER
00383 //==============================================================================
00390 class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceConvexImplicit 
00391 :   public ContactTracker {
00392 public:
00393 explicit HalfSpaceConvexImplicit
00394    (ContactGeometryTypeId typeOfConvexImplicitSurface) 
00395 :   ContactTracker(ContactGeometry::HalfSpace::classTypeId(),
00396                    typeOfConvexImplicitSurface) {}
00397 
00398 bool trackContact
00399    (const Contact&         priorStatus,
00400     const Transform& X_GS1, 
00401     const ContactGeometry& surface1, // the half-space
00402     const Transform& X_GS2, 
00403     const ContactGeometry& surface2, // the convex implicit surface
00404     Real                   cutoff,
00405     Contact&               currentStatus) const OVERRIDE_11;
00406 };
00407 
00408 
00409 //==============================================================================
00410 //               CONVEX IMPLICIT SURFACE PAIR CONTACT TRACKER
00411 //==============================================================================
00415 class SimTK_SIMMATH_EXPORT ContactTracker::ConvexImplicitPair 
00416 :   public ContactTracker {
00417 public:
00418 ConvexImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2) 
00419 :   ContactTracker(type1, type2) {}
00420 
00421 bool trackContact
00422    (const Contact&         priorStatus,
00423     const Transform& X_GS1, 
00424     const ContactGeometry& surface1,
00425     const Transform& X_GS2, 
00426     const ContactGeometry& surface2,
00427     Real                   cutoff,
00428     Contact&               currentStatus) const OVERRIDE_11;
00429 };
00430 
00431 
00432 //==============================================================================
00433 //                GENERAL IMPLICIT SURFACE PAIR CONTACT TRACKER
00434 //==============================================================================
00442 class SimTK_SIMMATH_EXPORT ContactTracker::GeneralImplicitPair 
00443 :   public ContactTracker {
00444 public:
00445 GeneralImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2) 
00446 :   ContactTracker(type1, type2) {}
00447 
00448 bool trackContact
00449    (const Contact&         priorStatus,
00450     const Transform& X_GS1, 
00451     const ContactGeometry& surface1,
00452     const Transform& X_GS2, 
00453     const ContactGeometry& surface2,
00454     Real                   cutoff,
00455     Contact&               currentStatus) const OVERRIDE_11;
00456 };
00457 
00458 } // namespace SimTK
00459 
00460 #endif // SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines