|
Simbody
3.4 (development)
|
|
Simbody
3.4 (development)
|
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-12 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 SphereSphere; 00067 class HalfSpaceTriangleMesh; 00068 class SphereTriangleMesh; 00069 class TriangleMeshTriangleMesh; 00070 class ConvexImplicitPair; 00071 class GeneralImplicitPair; 00072 00074 ContactTracker(ContactGeometryTypeId typeOfSurface1, 00075 ContactGeometryTypeId typeOfSurface2) 00076 : m_surfaceTypes(typeOfSurface1, typeOfSurface2) 00077 { 00078 } 00079 00082 const std::pair<ContactGeometryTypeId,ContactGeometryTypeId>& 00083 getContactGeometryTypeIds() const {return m_surfaceTypes;} 00084 00085 virtual ~ContactTracker() {} 00086 00092 virtual bool trackContact 00093 (const Contact& priorStatus, 00094 const Transform& X_GS1, 00095 const ContactGeometry& surface1, 00096 const Transform& X_GS2, 00097 const ContactGeometry& surface2, 00098 Real cutoff, 00099 Contact& currentStatus) const = 0; 00100 00107 virtual bool predictContact 00108 (const Contact& priorStatus, 00109 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00110 const ContactGeometry& surface1, 00111 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00112 const ContactGeometry& surface2, 00113 Real cutoff, 00114 Real intervalOfInterest, 00115 Contact& predictedStatus) const = 0; 00116 00122 virtual bool initializeContact 00123 (const Transform& X_GS1, const SpatialVec& V_GS1, 00124 const ContactGeometry& surface1, 00125 const Transform& X_GS2, const SpatialVec& V_GS2, 00126 const ContactGeometry& surface2, 00127 Real cutoff, 00128 Real intervalOfInterest, 00129 Contact& contactStatus) const = 0; 00130 00157 static bool refineImplicitPair 00158 (const ContactGeometry& shapeA, Vec3& pointP_A, // in/out 00159 const ContactGeometry& shapeB, Vec3& pointQ_B, // in/out 00160 const Transform& X_AB, Real accuracyRequested, 00161 Real& accuracyAchieved, int& numIterations); 00162 00164 static Vec6 findImplicitPairError 00165 (const ContactGeometry& shapeA, const Vec3& pointP, 00166 const ContactGeometry& shapeB, const Vec3& pointQ, 00167 const Transform& X_AB); 00168 00173 static Mat66 calcImplicitPairJacobian 00174 (const ContactGeometry& shapeA, const Vec3& pointP, 00175 const ContactGeometry& shapeB, const Vec3& pointQ, 00176 const Transform& X_AB, const Vec6& err0); 00177 00188 static bool estimateConvexImplicitPairContactUsingMPR 00189 (const ContactGeometry& shapeA, const ContactGeometry& shapeB, 00190 const Transform& X_AB, 00191 Vec3& pointP_A, Vec3& pointQ_B, UnitVec3& dirInA, 00192 int& numIterations); 00193 00194 00195 //-------------------------------------------------------------------------- 00196 private: 00197 // This tracker should be called only for surfaces of these two types, 00198 // in this order. 00199 std::pair<ContactGeometryTypeId,ContactGeometryTypeId> m_surfaceTypes; 00200 }; 00201 00202 00203 00204 //============================================================================== 00205 // HALFSPACE-SPHERE CONTACT TRACKER 00206 //============================================================================== 00209 class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceSphere 00210 : public ContactTracker { 00211 public: 00212 HalfSpaceSphere() 00213 : ContactTracker(ContactGeometry::HalfSpace::classTypeId(), 00214 ContactGeometry::Sphere::classTypeId()) {} 00215 00216 virtual ~HalfSpaceSphere() {} 00217 00218 virtual bool trackContact 00219 (const Contact& priorStatus, 00220 const Transform& X_GS1, 00221 const ContactGeometry& surface1, 00222 const Transform& X_GS2, 00223 const ContactGeometry& surface2, 00224 Real cutoff, 00225 Contact& currentStatus) const; 00226 00227 virtual bool predictContact 00228 (const Contact& priorStatus, 00229 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00230 const ContactGeometry& surface1, 00231 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00232 const ContactGeometry& surface2, 00233 Real cutoff, 00234 Real intervalOfInterest, 00235 Contact& predictedStatus) const; 00236 00237 virtual bool initializeContact 00238 (const Transform& X_GS1, const SpatialVec& V_GS1, 00239 const ContactGeometry& surface1, 00240 const Transform& X_GS2, const SpatialVec& V_GS2, 00241 const ContactGeometry& surface2, 00242 Real cutoff, 00243 Real intervalOfInterest, 00244 Contact& contactStatus) const; 00245 }; 00246 00247 00248 00249 //============================================================================== 00250 // HALFSPACE-ELLIPSOID CONTACT TRACKER 00251 //============================================================================== 00254 class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceEllipsoid 00255 : public ContactTracker { 00256 public: 00257 HalfSpaceEllipsoid() 00258 : ContactTracker(ContactGeometry::HalfSpace::classTypeId(), 00259 ContactGeometry::Ellipsoid::classTypeId()) {} 00260 00261 virtual ~HalfSpaceEllipsoid() {} 00262 00263 virtual bool trackContact 00264 (const Contact& priorStatus, 00265 const Transform& X_GS1, 00266 const ContactGeometry& surface1, 00267 const Transform& X_GS2, 00268 const ContactGeometry& surface2, 00269 Real cutoff, 00270 Contact& currentStatus) const; 00271 00272 virtual bool predictContact 00273 (const Contact& priorStatus, 00274 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00275 const ContactGeometry& surface1, 00276 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00277 const ContactGeometry& surface2, 00278 Real cutoff, 00279 Real intervalOfInterest, 00280 Contact& predictedStatus) const; 00281 00282 virtual bool initializeContact 00283 (const Transform& X_GS1, const SpatialVec& V_GS1, 00284 const ContactGeometry& surface1, 00285 const Transform& X_GS2, const SpatialVec& V_GS2, 00286 const ContactGeometry& surface2, 00287 Real cutoff, 00288 Real intervalOfInterest, 00289 Contact& contactStatus) const; 00290 }; 00291 00292 00293 00294 //============================================================================== 00295 // SPHERE-SPHERE CONTACT TRACKER 00296 //============================================================================== 00299 class SimTK_SIMMATH_EXPORT ContactTracker::SphereSphere 00300 : public ContactTracker { 00301 public: 00302 SphereSphere() 00303 : ContactTracker(ContactGeometry::Sphere::classTypeId(), 00304 ContactGeometry::Sphere::classTypeId()) {} 00305 00306 virtual ~SphereSphere() {} 00307 00308 virtual bool trackContact 00309 (const Contact& priorStatus, 00310 const Transform& X_GS1, 00311 const ContactGeometry& surface1, 00312 const Transform& X_GS2, 00313 const ContactGeometry& surface2, 00314 Real cutoff, 00315 Contact& currentStatus) const; 00316 00317 virtual bool predictContact 00318 (const Contact& priorStatus, 00319 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00320 const ContactGeometry& surface1, 00321 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00322 const ContactGeometry& surface2, 00323 Real cutoff, 00324 Real intervalOfInterest, 00325 Contact& predictedStatus) const; 00326 00327 virtual bool initializeContact 00328 (const Transform& X_GS1, const SpatialVec& V_GS1, 00329 const ContactGeometry& surface1, 00330 const Transform& X_GS2, const SpatialVec& V_GS2, 00331 const ContactGeometry& surface2, 00332 Real cutoff, 00333 Real intervalOfInterest, 00334 Contact& contactStatus) const; 00335 }; 00336 00337 00338 00339 //============================================================================== 00340 // HALFSPACE-TRIANGLE MESH CONTACT TRACKER 00341 //============================================================================== 00344 class SimTK_SIMMATH_EXPORT ContactTracker::HalfSpaceTriangleMesh 00345 : public ContactTracker { 00346 public: 00347 HalfSpaceTriangleMesh() 00348 : ContactTracker(ContactGeometry::HalfSpace::classTypeId(), 00349 ContactGeometry::TriangleMesh::classTypeId()) {} 00350 00351 virtual ~HalfSpaceTriangleMesh() {} 00352 00353 virtual bool trackContact 00354 (const Contact& priorStatus, 00355 const Transform& X_GS1, 00356 const ContactGeometry& surface1, // the half space 00357 const Transform& X_GS2, 00358 const ContactGeometry& surface2, // the mesh 00359 Real cutoff, 00360 Contact& currentStatus) const; 00361 00362 virtual bool predictContact 00363 (const Contact& priorStatus, 00364 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00365 const ContactGeometry& surface1, 00366 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00367 const ContactGeometry& surface2, 00368 Real cutoff, 00369 Real intervalOfInterest, 00370 Contact& predictedStatus) const; 00371 00372 virtual bool initializeContact 00373 (const Transform& X_GS1, const SpatialVec& V_GS1, 00374 const ContactGeometry& surface1, 00375 const Transform& X_GS2, const SpatialVec& V_GS2, 00376 const ContactGeometry& surface2, 00377 Real cutoff, 00378 Real intervalOfInterest, 00379 Contact& contactStatus) const; 00380 00381 private: 00382 void processBox(const ContactGeometry::TriangleMesh& mesh, 00383 const ContactGeometry::TriangleMesh::OBBTreeNode& node, 00384 const Transform& X_HM, const UnitVec3& hsNormal_M, 00385 Real hsFaceHeight_M, std::set<int>& insideFaces) const; 00386 void addAllTriangles(const ContactGeometry::TriangleMesh::OBBTreeNode& node, 00387 std::set<int>& insideFaces) const; 00388 }; 00389 00390 00391 00392 //============================================================================== 00393 // SPHERE - TRIANGLE MESH CONTACT TRACKER 00394 //============================================================================== 00397 class SimTK_SIMMATH_EXPORT ContactTracker::SphereTriangleMesh 00398 : public ContactTracker { 00399 public: 00400 SphereTriangleMesh() 00401 : ContactTracker(ContactGeometry::Sphere::classTypeId(), 00402 ContactGeometry::TriangleMesh::classTypeId()) {} 00403 00404 virtual ~SphereTriangleMesh() {} 00405 00406 virtual bool trackContact 00407 (const Contact& priorStatus, 00408 const Transform& X_GS1, 00409 const ContactGeometry& surface1, // the sphere 00410 const Transform& X_GS2, 00411 const ContactGeometry& surface2, // the mesh 00412 Real cutoff, 00413 Contact& currentStatus) const; 00414 00415 virtual bool predictContact 00416 (const Contact& priorStatus, 00417 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00418 const ContactGeometry& surface1, 00419 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00420 const ContactGeometry& surface2, 00421 Real cutoff, 00422 Real intervalOfInterest, 00423 Contact& predictedStatus) const; 00424 00425 virtual bool initializeContact 00426 (const Transform& X_GS1, const SpatialVec& V_GS1, 00427 const ContactGeometry& surface1, 00428 const Transform& X_GS2, const SpatialVec& V_GS2, 00429 const ContactGeometry& surface2, 00430 Real cutoff, 00431 Real intervalOfInterest, 00432 Contact& contactStatus) const; 00433 00434 private: 00435 void processBox 00436 (const ContactGeometry::TriangleMesh& mesh, 00437 const ContactGeometry::TriangleMesh::OBBTreeNode& node, 00438 const Vec3& center_M, Real radius2, 00439 std::set<int>& insideFaces) const ; 00440 }; 00441 00442 00443 00444 //============================================================================== 00445 // TRIANGLE MESH - TRIANGLE MESH CONTACT TRACKER 00446 //============================================================================== 00449 class SimTK_SIMMATH_EXPORT ContactTracker::TriangleMeshTriangleMesh 00450 : public ContactTracker { 00451 public: 00452 TriangleMeshTriangleMesh() 00453 : ContactTracker(ContactGeometry::TriangleMesh::classTypeId(), 00454 ContactGeometry::TriangleMesh::classTypeId()) {} 00455 00456 virtual ~TriangleMeshTriangleMesh() {} 00457 00458 virtual bool trackContact 00459 (const Contact& priorStatus, 00460 const Transform& X_GS1, 00461 const ContactGeometry& surface1, // mesh1 00462 const Transform& X_GS2, 00463 const ContactGeometry& surface2, // mesh2 00464 Real cutoff, 00465 Contact& currentStatus) const; 00466 00467 virtual bool predictContact 00468 (const Contact& priorStatus, 00469 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00470 const ContactGeometry& surface1, 00471 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00472 const ContactGeometry& surface2, 00473 Real cutoff, 00474 Real intervalOfInterest, 00475 Contact& predictedStatus) const; 00476 00477 virtual bool initializeContact 00478 (const Transform& X_GS1, const SpatialVec& V_GS1, 00479 const ContactGeometry& surface1, 00480 const Transform& X_GS2, const SpatialVec& V_GS2, 00481 const ContactGeometry& surface2, 00482 Real cutoff, 00483 Real intervalOfInterest, 00484 Contact& contactStatus) const; 00485 00486 private: 00487 void findIntersectingFaces 00488 (const ContactGeometry::TriangleMesh& mesh1, 00489 const ContactGeometry::TriangleMesh& mesh2, 00490 const ContactGeometry::TriangleMesh::OBBTreeNode& node1, 00491 const ContactGeometry::TriangleMesh::OBBTreeNode& node2, 00492 const OrientedBoundingBox& node2Bounds_M1, 00493 const Transform& X_M1M2, 00494 std::set<int>& insideFaces1, 00495 std::set<int>& insideFaces2) const; 00496 void findBuriedFaces 00497 (const ContactGeometry::TriangleMesh& mesh, 00498 const ContactGeometry::TriangleMesh& otherMesh, 00499 const Transform& X_OM, 00500 std::set<int>& insideFaces) const; 00501 void tagFaces(const ContactGeometry::TriangleMesh& mesh, 00502 Array_<int>& faceType, 00503 std::set<int>& triangles, 00504 int index, 00505 int depth) const; 00506 }; 00507 00508 00509 //============================================================================== 00510 // CONVEX IMPLICIT SURFACE PAIR CONTACT TRACKER 00511 //============================================================================== 00515 class SimTK_SIMMATH_EXPORT ContactTracker::ConvexImplicitPair 00516 : public ContactTracker { 00517 public: 00518 ConvexImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2) 00519 : ContactTracker(type1, type2) {} 00520 00521 virtual ~ConvexImplicitPair() {} 00522 00523 virtual bool trackContact 00524 (const Contact& priorStatus, 00525 const Transform& X_GS1, 00526 const ContactGeometry& surface1, 00527 const Transform& X_GS2, 00528 const ContactGeometry& surface2, 00529 Real cutoff, 00530 Contact& currentStatus) const; 00531 00532 virtual bool predictContact 00533 (const Contact& priorStatus, 00534 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00535 const ContactGeometry& surface1, 00536 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00537 const ContactGeometry& surface2, 00538 Real cutoff, 00539 Real intervalOfInterest, 00540 Contact& predictedStatus) const; 00541 00542 virtual bool initializeContact 00543 (const Transform& X_GS1, const SpatialVec& V_GS1, 00544 const ContactGeometry& surface1, 00545 const Transform& X_GS2, const SpatialVec& V_GS2, 00546 const ContactGeometry& surface2, 00547 Real cutoff, 00548 Real intervalOfInterest, 00549 Contact& contactStatus) const; 00550 }; 00551 00552 00553 //============================================================================== 00554 // GENERAL IMPLICIT SURFACE PAIR CONTACT TRACKER 00555 //============================================================================== 00563 class SimTK_SIMMATH_EXPORT ContactTracker::GeneralImplicitPair 00564 : public ContactTracker { 00565 public: 00566 GeneralImplicitPair(ContactGeometryTypeId type1, ContactGeometryTypeId type2) 00567 : ContactTracker(type1, type2) {} 00568 00569 virtual ~GeneralImplicitPair() {} 00570 00571 virtual bool trackContact 00572 (const Contact& priorStatus, 00573 const Transform& X_GS1, 00574 const ContactGeometry& surface1, 00575 const Transform& X_GS2, 00576 const ContactGeometry& surface2, 00577 Real cutoff, 00578 Contact& currentStatus) const; 00579 00580 virtual bool predictContact 00581 (const Contact& priorStatus, 00582 const Transform& X_GS1, const SpatialVec& V_GS1, const SpatialVec& A_GS1, 00583 const ContactGeometry& surface1, 00584 const Transform& X_GS2, const SpatialVec& V_GS2, const SpatialVec& A_GS2, 00585 const ContactGeometry& surface2, 00586 Real cutoff, 00587 Real intervalOfInterest, 00588 Contact& predictedStatus) const; 00589 00590 virtual bool initializeContact 00591 (const Transform& X_GS1, const SpatialVec& V_GS1, 00592 const ContactGeometry& surface1, 00593 const Transform& X_GS2, const SpatialVec& V_GS2, 00594 const ContactGeometry& surface2, 00595 Real cutoff, 00596 Real intervalOfInterest, 00597 Contact& contactStatus) const; 00598 }; 00599 00600 } // namespace SimTK 00601 00602 #endif // SimTK_SIMMATH_CONTACT_TRACKER_SUBSYSTEM_H_
1.7.6.1 
