Simbody
3.5
|
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_