Extras/PhysicsEffects/src/base_level/collision/pfx_contact_capsule_sphere.cpp

   1 /*\r
   2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.\r
   3 All rights reserved.\r
   4 \r
   5 Physics Effects is open software; you can redistribute it and/or\r
   6 modify it under the terms of the BSD License.\r
   7 \r
   8 Physics Effects is distributed in the hope that it will be useful,\r
   9 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
  11 See the BSD License for more details.\r
  12 \r
  13 A copy of the BSD License is distributed with\r
  14 Physics Effects under the filename: physics_effects_license.txt\r
  15 */\r
  16 \r
  17 #include "../../../include/physics_effects/base_level/base/pfx_vec_utils.h"\r
  18 #include "../../../include/physics_effects/base_level/collision/pfx_capsule.h"\r
  19 #include "../../../include/physics_effects/base_level/collision/pfx_sphere.h"\r
  20 #include "pfx_contact_capsule_sphere.h"\r
  21 \r
  22 namespace sce {\r
  23 namespace PhysicsEffects {\r
  24 \r
  25 inline\r
  26 void\r
  27 segmentPointClosestPoints(\r
  28         PfxVector3& ptsVector,\r
  29         PfxVector3& offsetA,\r
  30         PfxFloat& tA,\r
  31         const PfxVector3 &translation,\r
  32         const PfxVector3 &dirA, PfxFloat hLenA )\r
  33 {\r
  34         // compute the parameters of the closest points on each line segment\r
  35 \r
  36         tA = dot(dirA,translation);\r
  37 \r
  38         if ( tA < -hLenA )\r
  39                 tA = -hLenA;\r
  40         else if ( tA > hLenA )\r
  41                 tA = hLenA;\r
  42 \r
  43         // compute the closest point on segment relative to its center.\r
  44 \r
  45         offsetA = dirA * tA;\r
  46         ptsVector = translation - offsetA;\r
  47 }\r
  48 \r
  49 inline\r
  50 void\r
  51 segmentPointNormal(     PfxVector3& normal, const  PfxVector3 &ptsVector )\r
  52 {\r
  53         // compute the unit direction vector between the closest points.\r
  54         // with convex objects, you want the unit direction providing the largest gap between the\r
  55         // objects when they're projected onto it.  So, if you have a few candidates covering different\r
  56         // configurations of the objects, you can compute them all, test the gaps and pick best axis\r
  57         // based on this.  Some directions might be degenerate, and the normalized() function tests for\r
  58         // degeneracy and returns an arbitrary unit vector in that case.\r
  59 \r
  60         // closest points vector\r
  61 \r
  62         normal = pfxSafeNormalize(ptsVector);\r
  63 }\r
  64 \r
  65 PfxFloat pfxContactCapsuleSphere(\r
  66         PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,\r
  67         void *shapeA,const PfxTransform3 &transformA,\r
  68         void *shapeB,const PfxTransform3 &transformB,\r
  69         PfxFloat distanceThreshold)\r
  70 {\r
  71         PfxCapsule &capsuleA = *((PfxCapsule*)shapeA);\r
  72         PfxSphere &sphereB = *((PfxSphere*)shapeB);\r
  73 \r
  74         PfxVector3 directionA = transformA.getUpper3x3().getCol0();\r
  75         PfxVector3 translationA = transformA.getTranslation();\r
  76         PfxVector3 translationB = transformB.getTranslation();\r
  77 \r
  78         // translation between centers of capsule and sphere\r
  79 \r
  80         PfxVector3 translation = translationB - translationA;\r
  81 \r
  82         // compute the closest point on the capsule line segment to the sphere center\r
  83 \r
  84         PfxVector3 ptsVector;\r
  85         PfxVector3 offsetA;\r
  86         PfxFloat tA;\r
  87 \r
  88         segmentPointClosestPoints( ptsVector, offsetA, tA, translation, directionA, capsuleA.m_halfLen );\r
  89 \r
  90         PfxFloat distance = length(ptsVector) - capsuleA.m_radius - sphereB.m_radius;\r
  91 \r
  92         if ( distance > distanceThreshold )\r
  93                 return distance;\r
  94 \r
  95         // compute the contact normal\r
  96 \r
  97         segmentPointNormal( normal, ptsVector );\r
  98 \r
  99         // compute points on capsule and sphere\r
 100 \r
 101         pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) );\r
 102         pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( -normal * sphereB.m_radius ) );\r
 103 \r
 104         return distance;\r
 105 }\r
 106 \r
 107 } //namespace PhysicsEffects\r
 108 } //namespace sce\r