Extras/PhysicsEffects/src/base_level/collision/pfx_contact_cache.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 "pfx_contact_cache.h"\r
  18 \r
  19 namespace sce {\r
  20 namespace PhysicsEffects {\r
  21 \r
  22 #define SCE_PFX_CONTACT_SAME_POINT 0.01f\r
  23 \r
  24 int PfxContactCache::findNearestContactPoint(const PfxPoint3 &newPoint,const PfxVector3 &newNormal)\r
  25 {\r
  26         int nearestIdx = -1;\r
  27         PfxFloat minDiff = SCE_PFX_CONTACT_SAME_POINT;\r
  28         for(PfxUInt32 i=0;i<m_numContacts;i++) {\r
  29                 PfxVector3 dist = m_cachedContactPoints[i].m_localPointA-newPoint;\r
  30                 PfxFloat diff = lengthSqr(dist);\r
  31                 if(diff < minDiff && dot(newNormal,m_cachedContactPoints[i].m_normal) > 0.99f) {\r
  32                         minDiff = diff;\r
  33                         nearestIdx = i;\r
  34                 }\r
  35         }\r
  36         return nearestIdx;\r
  37 }\r
  38 \r
  39 int PfxContactCache::sort4ContactPoints(const PfxPoint3 &newCP,PfxFloat newDistance)\r
  40 {\r
  41         int maxPenetrationIndex = -1;\r
  42         PfxFloat maxPenetration = newDistance;\r
  43 \r
  44         // 最も深い衝突点は排除対象からはずす\r
  45         for(int i=0;i<SCE_PFX_MAX_CACHED_CONTACT_POINTS;i++) {\r
  46                 if(m_cachedContactPoints[i].m_distance < maxPenetration) {\r
  47                         maxPenetrationIndex = i;\r
  48                         maxPenetration = m_cachedContactPoints[i].m_distance;\r
  49                 }\r
  50         }\r
  51         \r
  52         PfxFloat res[4] = {0.0f};\r
  53         \r
  54         // 各点を除いたときの衝突点が作る面積のうち、最も大きくなるものを選択\r
  55         PfxVector3 newp(newCP);\r
  56         PfxVector3 p[4];\r
  57         p[0] = (PfxVector3)m_cachedContactPoints[0].m_localPointA;\r
  58         p[1] = (PfxVector3)m_cachedContactPoints[1].m_localPointA;\r
  59         p[2] = (PfxVector3)m_cachedContactPoints[2].m_localPointA;\r
  60         p[3] = (PfxVector3)m_cachedContactPoints[3].m_localPointA;\r
  61 \r
  62         if(maxPenetrationIndex != 0) {\r
  63                 PfxVector3 a0 = newp-p[1];\r
  64                 PfxVector3 b0 = p[3]-p[2];\r
  65                 res[0] = lengthSqr(cross(a0,b0));\r
  66         }\r
  67  \r
  68         if(maxPenetrationIndex != 1) {\r
  69                 PfxVector3 a1 = newp-p[0];\r
  70                 PfxVector3 b1 = p[3]-p[2];\r
  71                 res[1] = lengthSqr(cross(a1,b1));\r
  72         }\r
  73 \r
  74         if(maxPenetrationIndex != 2) {\r
  75                 PfxVector3 a2 = newp-p[0];\r
  76                 PfxVector3 b2 = p[3]-p[1];\r
  77                 res[2] = lengthSqr(cross(a2,b2));\r
  78         }\r
  79 \r
  80         if(maxPenetrationIndex != 3) {\r
  81                 PfxVector3 a3 = newp-p[0];\r
  82                 PfxVector3 b3 = p[2]-p[1];\r
  83                 res[3] = lengthSqr(cross(a3,b3));\r
  84         }\r
  85 \r
  86         int maxIndex = 0;\r
  87         PfxFloat maxVal = res[0];\r
  88 \r
  89         if (res[1] > maxVal) {\r
  90                 maxIndex = 1;\r
  91                 maxVal = res[1];\r
  92         }\r
  93 \r
  94         if (res[2] > maxVal) {\r
  95                 maxIndex = 2;\r
  96                 maxVal = res[2];\r
  97         }\r
  98 \r
  99         if (res[3] > maxVal) {\r
 100                 maxIndex = 3;\r
 101                 maxVal = res[3];\r
 102         }\r
 103 \r
 104         return maxIndex;\r
 105 }\r
 106 \r
 107 void PfxContactCache::addContactPoint(\r
 108                 PfxFloat newDistance,\r
 109                 const PfxVector3 &newNormal, // world coords\r
 110                 const PfxPoint3 &newPointA, // local to the objectA\r
 111                 const PfxPoint3 &newPointB, // local to the objectB\r
 112                 PfxSubData subData)\r
 113 {\r
 114         int id = findNearestContactPoint(newPointA,newNormal);\r
 115 \r
 116         if(id < 0 && m_numContacts < SCE_PFX_MAX_CACHED_CONTACT_POINTS) {\r
 117                 // 衝突点を新規追加\r
 118                 id = m_numContacts++;\r
 119                 m_cachedContactPoints[id].reset();\r
 120         }\r
 121         else if(id < 0){\r
 122                 // ソート\r
 123                 id = sort4ContactPoints(newPointA,newDistance);\r
 124                 m_cachedContactPoints[id].reset();\r
 125         }\r
 126 \r
 127         m_cachedContactPoints[id].m_distance = newDistance;\r
 128         m_cachedContactPoints[id].m_subData = subData;\r
 129         m_cachedContactPoints[id].m_normal = newNormal;\r
 130         m_cachedContactPoints[id].m_localPointA = newPointA;\r
 131         m_cachedContactPoints[id].m_localPointB = newPointB;\r
 132 }\r
 133 \r
 134 void PfxContactCache::addContactPoint(const PfxCachedContactPoint &cp)\r
 135 {\r
 136         PfxPoint3 pA = cp.m_localPointA;\r
 137         \r
 138         int id = findNearestContactPoint(pA,cp.m_normal);\r
 139         \r
 140         if(id >= 0) {\r
 141                 if(m_cachedContactPoints[id].m_distance > cp.m_distance) {\r
 142                         // 同一点を発見、衝突点情報を更新\r
 143                         m_cachedContactPoints[id].m_distance = cp.m_distance;\r
 144                         m_cachedContactPoints[id].m_normal = cp.m_normal;\r
 145                         m_cachedContactPoints[id].m_localPointA = cp.m_localPointA;\r
 146                         m_cachedContactPoints[id].m_localPointB = cp.m_localPointB;\r
 147                 }\r
 148         }\r
 149         else if(m_numContacts < SCE_PFX_MAX_CACHED_CONTACT_POINTS) {\r
 150                 // 衝突点を新規追加\r
 151                 m_cachedContactPoints[m_numContacts++] = cp;\r
 152         }\r
 153         else {\r
 154                 // ソート\r
 155                 id = sort4ContactPoints(pA,cp.m_distance);\r
 156                 \r
 157                 // コンタクトポイント入れ替え\r
 158                 m_cachedContactPoints[id] = cp;\r
 159         }\r
 160 }\r
 161 \r
 162 } //namespace PhysicsEffects\r
 163 } //namespace sce\r
 164 \r