Extras/PhysicsEffects/src/base_level/collision/pfx_contact_capsule_capsule.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 "pfx_contact_capsule_capsule.h"\r
  20 \r
  21 namespace sce {\r
  22 namespace PhysicsEffects {\r
  23 \r
  24 inline\r
  25 void\r
  26 segmentsClosestPoints(\r
  27         PfxVector3& ptsVector,\r
  28         PfxVector3& offsetA,\r
  29         PfxVector3& offsetB,\r
  30         PfxFloat& tA, PfxFloat& tB,\r
  31         const PfxVector3 &translation,\r
  32         const PfxVector3 &dirA, PfxFloat hlenA,\r
  33         const PfxVector3 &dirB, PfxFloat hlenB )\r
  34 {\r
  35         // compute the parameters of the closest points on each line segment\r
  36 \r
  37         PfxFloat dirA_dot_dirB = dot(dirA,dirB);\r
  38         PfxFloat dirA_dot_trans = dot(dirA,translation);\r
  39         PfxFloat dirB_dot_trans = dot(dirB,translation);\r
  40 \r
  41         PfxFloat denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;\r
  42 \r
  43         if ( denom == 0.0f ) {\r
  44                 tA = 0.0f;\r
  45         } else {\r
  46                 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;\r
  47                 if ( tA < -hlenA )\r
  48                         tA = -hlenA;\r
  49                 else if ( tA > hlenA )\r
  50                         tA = hlenA;\r
  51         }\r
  52 \r
  53         tB = tA * dirA_dot_dirB - dirB_dot_trans;\r
  54 \r
  55         if ( tB < -hlenB ) {\r
  56                 tB = -hlenB;\r
  57                 tA = tB * dirA_dot_dirB + dirA_dot_trans;\r
  58 \r
  59                 if ( tA < -hlenA )\r
  60                         tA = -hlenA;\r
  61                 else if ( tA > hlenA )\r
  62                         tA = hlenA;\r
  63         } else if ( tB > hlenB ) {\r
  64                 tB = hlenB;\r
  65                 tA = tB * dirA_dot_dirB + dirA_dot_trans;\r
  66 \r
  67                 if ( tA < -hlenA )\r
  68                         tA = -hlenA;\r
  69                 else if ( tA > hlenA )\r
  70                         tA = hlenA;\r
  71         }\r
  72 \r
  73         // compute the closest points relative to segment centers.\r
  74 \r
  75         offsetA = dirA * tA;\r
  76         offsetB = dirB * tB;\r
  77 \r
  78         ptsVector = translation - offsetA + offsetB;\r
  79 }\r
  80 \r
  81 inline\r
  82 void\r
  83 segmentsNormal( PfxVector3& normal, const PfxVector3 &ptsVector )\r
  84 {\r
  85         // compute the unit direction vector between the closest points.\r
  86         // with convex objects, you want the unit direction providing the largest gap between the\r
  87         // objects when they're projected onto it.  So, if you have a few candidates covering different\r
  88         // configurations of the objects, you can compute them all, test the gaps and pick best axis\r
  89         // based on this.  Some directions might be degenerate, and the normalized() function tests for\r
  90         // degeneracy and returns an arbitrary unit vector in that case.\r
  91 \r
  92         PfxVector3 testDir;\r
  93 \r
  94         // closest points vector\r
  95 \r
  96         normal = pfxSafeNormalize(ptsVector);\r
  97 }\r
  98 \r
  99 PfxFloat pfxContactCapsuleCapsule(\r
 100         PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,\r
 101         void *shapeA,const PfxTransform3 &transformA,\r
 102         void *shapeB,const PfxTransform3 &transformB,\r
 103         PfxFloat distanceThreshold)\r
 104 {\r
 105         PfxCapsule &capsuleA = *((PfxCapsule*)shapeA);\r
 106         PfxCapsule &capsuleB = *((PfxCapsule*)shapeB);\r
 107 \r
 108         PfxVector3 directionA = transformA.getUpper3x3().getCol0();\r
 109         PfxVector3 translationA = transformA.getTranslation();\r
 110         PfxVector3 directionB = transformB.getUpper3x3().getCol0();\r
 111         PfxVector3 translationB = transformB.getTranslation();\r
 112 \r
 113         // translation between centers\r
 114 \r
 115         PfxVector3 translation = translationB - translationA;\r
 116 \r
 117         // compute the closest points of the capsule line segments\r
 118 \r
 119         PfxVector3 ptsVector;           // the vector between the closest points\r
 120         PfxVector3 offsetA, offsetB;    // offsets from segment centers to their closest points\r
 121         PfxFloat tA, tB;              // parameters on line segment\r
 122 \r
 123         segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,\r
 124                                                    directionA, capsuleA.m_halfLen, directionB, capsuleB.m_halfLen );\r
 125 \r
 126         PfxFloat distance = length(ptsVector) - capsuleA.m_radius - capsuleB.m_radius;\r
 127 \r
 128         if ( distance > distanceThreshold )\r
 129                 return distance;\r
 130 \r
 131         // compute the contact normal\r
 132 \r
 133         segmentsNormal( normal, ptsVector );\r
 134 \r
 135         // compute points on capsules\r
 136 \r
 137         pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( offsetA + normal * capsuleA.m_radius ) );\r
 138         pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( offsetB - normal * capsuleB.m_radius ) );\r
 139 \r
 140         return distance;\r
 141 }\r
 142 \r
 143 } //namespace PhysicsEffects\r
 144 } //namespace sce\r