Extras/PhysicsEffects/src/base_level/collision/pfx_contact_sphere_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/collision/pfx_sphere.h"\r
  18 #include "pfx_contact_sphere_sphere.h"\r
  19 \r
  20 namespace sce {\r
  21 namespace PhysicsEffects {\r
  22 \r
  23 const PfxFloat lenSqrTol = 1.0e-30f;\r
  24 \r
  25 PfxFloat pfxContactSphereSphere(\r
  26         PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,\r
  27         void *shapeA,const PfxTransform3 &transformA,\r
  28         void *shapeB,const PfxTransform3 &transformB,\r
  29         PfxFloat distanceThreshold)\r
  30 {\r
  31         PfxSphere &sphereA = *((PfxSphere*)shapeA);\r
  32         PfxSphere &sphereB = *((PfxSphere*)shapeB);\r
  33 \r
  34         PfxVector3 direction(0.0f);\r
  35 \r
  36         PfxVector3 translationA = transformA.getTranslation();\r
  37         PfxVector3 translationB = transformB.getTranslation();\r
  38 \r
  39         // get the offset vector between sphere centers\r
  40 \r
  41         PfxVector3 offsetAB;\r
  42 \r
  43         offsetAB = translationB - translationA;\r
  44 \r
  45         // normalize the offset to compute the direction vector\r
  46 \r
  47         PfxFloat distSqr = dot(offsetAB,offsetAB);\r
  48         PfxFloat dist = sqrtf(distSqr);\r
  49         PfxFloat sphereDist = dist - sphereA.m_radius - sphereB.m_radius;\r
  50 \r
  51         if ( sphereDist > distanceThreshold ) {\r
  52                 return sphereDist;\r
  53         }\r
  54 \r
  55         if ( distSqr > lenSqrTol ) {\r
  56                 PfxFloat distInv = 1.0f / dist;\r
  57 \r
  58                 direction = offsetAB * distInv;\r
  59         } else {\r
  60                 direction = PfxVector3(0.0f, 0.0f, 1.0f);\r
  61         }\r
  62 \r
  63         normal = direction;\r
  64 \r
  65         // compute the points on the spheres, in world space\r
  66 \r
  67         pointA = PfxPoint3( transpose(transformA.getUpper3x3()) * ( direction * sphereA.m_radius ) );\r
  68         pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( -direction * sphereB.m_radius ) );\r
  69 \r
  70         // return the distance between the spheres\r
  71 \r
  72         return sphereDist;\r
  73 }\r
  74 \r
  75 } //namespace PhysicsEffects\r
  76 } //namespace sce\r