Extras/PhysicsEffects/src/base_level/collision/pfx_contact_manifold.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_contact_manifold.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 #define SCE_PFX_CONTACT_THRESHOLD_NORMAL        0.01f   // 衝突点の閾値（法線方向）\r
  24 #define SCE_PFX_CONTACT_THRESHOLD_TANGENT       0.002f  // 衝突点の閾値（平面上）\r
  25 \r
  26 int PfxContactManifold::findNearestContactPoint(const PfxPoint3 &newPoint,const PfxVector3 &newNormal)\r
  27 {\r
  28         int nearestIdx = -1;\r
  29         PfxFloat minDiff = SCE_PFX_CONTACT_SAME_POINT;\r
  30         for(int i=0;i<m_numContacts;i++) {\r
  31                 PfxFloat diff = lengthSqr(pfxReadVector3(m_contactPoints[i].m_localPointA) - PfxVector3(newPoint));\r
  32                 if(diff < minDiff && dot(newNormal,pfxReadVector3(m_contactPoints[i].m_constraintRow[0].m_normal)) > 0.99f) {\r
  33                         minDiff = diff;\r
  34                         nearestIdx = i;\r
  35                 }\r
  36         }\r
  37         return nearestIdx;\r
  38 }\r
  39 \r
  40 int PfxContactManifold::sort4ContactPoints(const PfxPoint3 &newCP,PfxFloat newDistance)\r
  41 {\r
  42         int maxPenetrationIndex = -1;\r
  43         PfxFloat maxPenetration = newDistance;\r
  44 \r
  45         // 最も深い衝突点は排除対象からはずす\r
  46         for(int i=0;i<SCE_PFX_NUMCONTACTS_PER_BODIES;i++) {\r
  47                 if(m_contactPoints[i].m_distance < maxPenetration) {\r
  48                         maxPenetrationIndex = i;\r
  49                         maxPenetration = m_contactPoints[i].m_distance;\r
  50                 }\r
  51         }\r
  52         \r
  53         PfxFloat res[4] = {0.0f};\r
  54         \r
  55         // 各点を除いたときの衝突点が作る面積のうち、最も大きくなるものを選択\r
  56         PfxVector3 newp(newCP);\r
  57         PfxVector3 p[4];\r
  58         p[0] = pfxReadVector3(m_contactPoints[0].m_localPointA);\r
  59         p[1] = pfxReadVector3(m_contactPoints[1].m_localPointA);\r
  60         p[2] = pfxReadVector3(m_contactPoints[2].m_localPointA);\r
  61         p[3] = pfxReadVector3(m_contactPoints[3].m_localPointA);\r
  62 \r
  63         if(maxPenetrationIndex != 0) {\r
  64                 PfxVector3 a0 = newp-p[1];\r
  65                 PfxVector3 b0 = p[3]-p[2];\r
  66                 res[0] = lengthSqr(cross(a0,b0));\r
  67         }\r
  68  \r
  69         if(maxPenetrationIndex != 1) {\r
  70                 PfxVector3 a1 = newp-p[0];\r
  71                 PfxVector3 b1 = p[3]-p[2];\r
  72                 res[1] = lengthSqr(cross(a1,b1));\r
  73         }\r
  74 \r
  75         if(maxPenetrationIndex != 2) {\r
  76                 PfxVector3 a2 = newp-p[0];\r
  77                 PfxVector3 b2 = p[3]-p[1];\r
  78                 res[2] = lengthSqr(cross(a2,b2));\r
  79         }\r
  80 \r
  81         if(maxPenetrationIndex != 3) {\r
  82                 PfxVector3 a3 = newp-p[0];\r
  83                 PfxVector3 b3 = p[2]-p[1];\r
  84                 res[3] = lengthSqr(cross(a3,b3));\r
  85         }\r
  86 \r
  87         int maxIndex = 0;\r
  88         PfxFloat maxVal = res[0];\r
  89 \r
  90         if (res[1] > maxVal) {\r
  91                 maxIndex = 1;\r
  92                 maxVal = res[1];\r
  93         }\r
  94 \r
  95         if (res[2] > maxVal) {\r
  96                 maxIndex = 2;\r
  97                 maxVal = res[2];\r
  98         }\r
  99 \r
 100         if (res[3] > maxVal) {\r
 101                 maxIndex = 3;\r
 102                 maxVal = res[3];\r
 103         }\r
 104 \r
 105         return maxIndex;\r
 106 }\r
 107 \r
 108 void PfxContactManifold::addContactPoint(\r
 109                 PfxFloat newDistance,\r
 110                 const PfxVector3 &newNormal, // world coords\r
 111                 const PfxPoint3 &newPointA, // local to the objectA\r
 112                 const PfxPoint3 &newPointB, // local to the objectB\r
 113                 PfxSubData subData)\r
 114 {\r
 115         int id = findNearestContactPoint(newPointA,newNormal);\r
 116 \r
 117         if(id < 0 && m_numContacts < SCE_PFX_NUMCONTACTS_PER_BODIES) {\r
 118                 // 衝突点を新規追加\r
 119                 id = m_numContacts++;\r
 120                 m_contactPoints[id].reset();\r
 121         }\r
 122         else if(id < 0){\r
 123                 // ソート\r
 124                 id = sort4ContactPoints(newPointA,newDistance);\r
 125                 m_contactPoints[id].reset();\r
 126         }\r
 127 \r
 128         m_contactPoints[id].m_distance = newDistance;\r
 129         m_contactPoints[id].m_subData = subData;\r
 130         pfxStorePoint3(newPointA,m_contactPoints[id].m_localPointA);\r
 131         pfxStorePoint3(newPointB,m_contactPoints[id].m_localPointB);\r
 132         pfxStoreVector3(newNormal,m_contactPoints[id].m_constraintRow[0].m_normal);\r
 133 }\r
 134 \r
 135 void PfxContactManifold::addContactPoint(const PfxContactPoint &cp)\r
 136 {\r
 137         PfxPoint3 pA = pfxReadPoint3(cp.m_localPointA);\r
 138         \r
 139         int id = findNearestContactPoint(pA,pfxReadVector3(cp.m_constraintRow[0].m_normal));\r
 140         \r
 141         if(id >= 0) {\r
 142 #if 1\r
 143                 PfxVector3 nml1(pfxReadVector3(m_contactPoints[id].m_constraintRow[0].m_normal));\r
 144                 PfxVector3 nml2(pfxReadVector3(cp.m_constraintRow[0].m_normal));\r
 145                 if(fabsf(dot(nml1,nml2)) > 0.99f ) {\r
 146                         // 同一点を発見、蓄積された情報を継続\r
 147                         m_contactPoints[id].m_distance = cp.m_distance;\r
 148                         m_contactPoints[id].m_localPointA[0] = cp.m_localPointA[0];\r
 149                         m_contactPoints[id].m_localPointA[1] = cp.m_localPointA[1];\r
 150                         m_contactPoints[id].m_localPointA[2] = cp.m_localPointA[2];\r
 151                         m_contactPoints[id].m_localPointB[0] = cp.m_localPointB[0];\r
 152                         m_contactPoints[id].m_localPointB[1] = cp.m_localPointB[1];\r
 153                         m_contactPoints[id].m_localPointB[2] = cp.m_localPointB[2];\r
 154                         m_contactPoints[id].m_constraintRow[0].m_normal[0] = cp.m_constraintRow[0].m_normal[0];\r
 155                         m_contactPoints[id].m_constraintRow[0].m_normal[1] = cp.m_constraintRow[0].m_normal[1];\r
 156                         m_contactPoints[id].m_constraintRow[0].m_normal[2] = cp.m_constraintRow[0].m_normal[2];\r
 157                 }\r
 158                 else {\r
 159                         // 同一点ではあるが法線が違うため更新\r
 160                         m_contactPoints[id] = cp;\r
 161                 }\r
 162 #else\r
 163                 if(m_contactPoints[id].m_distance > cp.m_distance) {\r
 164                         // 同一点を発見、衝突点情報を更新\r
 165                         m_contactPoints[id].m_distance = cp.m_distance;\r
 166                         m_contactPoints[id].m_localPointA[0] = cp.m_localPointA[0];\r
 167                         m_contactPoints[id].m_localPointA[1] = cp.m_localPointA[1];\r
 168                         m_contactPoints[id].m_localPointA[2] = cp.m_localPointA[2];\r
 169                         m_contactPoints[id].m_localPointB[0] = cp.m_localPointB[0];\r
 170                         m_contactPoints[id].m_localPointB[1] = cp.m_localPointB[1];\r
 171                         m_contactPoints[id].m_localPointB[2] = cp.m_localPointB[2];\r
 172                         m_contactPoints[id].m_constraintRow[0].m_normal[0] = cp.m_constraintRow[0].m_normal[0];\r
 173                         m_contactPoints[id].m_constraintRow[0].m_normal[1] = cp.m_constraintRow[0].m_normal[1];\r
 174                         m_contactPoints[id].m_constraintRow[0].m_normal[2] = cp.m_constraintRow[0].m_normal[2];\r
 175                 }\r
 176 #endif\r
 177         }\r
 178         else if(m_numContacts < SCE_PFX_NUMCONTACTS_PER_BODIES) {\r
 179                 // 衝突点を新規追加\r
 180                 m_contactPoints[m_numContacts++] = cp;\r
 181         }\r
 182         else {\r
 183                 // ソート\r
 184                 id = sort4ContactPoints(pA,cp.m_distance);\r
 185                 \r
 186                 // コンタクトポイント入れ替え\r
 187                 m_contactPoints[id] = cp;\r
 188         }\r
 189 }\r
 190 \r
 191 void PfxContactManifold::merge(const PfxContactManifold &contact)\r
 192 {\r
 193         SCE_PFX_ALWAYS_ASSERT(m_rigidBodyIdA == contact.getRigidBodyIdA());\r
 194         SCE_PFX_ALWAYS_ASSERT(m_rigidBodyIdB == contact.getRigidBodyIdB());\r
 195 \r
 196         for(int i=0;i<contact.getNumContacts();i++) {\r
 197                 addContactPoint(contact.getContactPoint(i));\r
 198         }\r
 199 }\r
 200 \r
 201 void PfxContactManifold::refresh(const PfxVector3 &pA,const PfxQuat &qA,const PfxVector3 &pB,const PfxQuat &qB)\r
 202 {\r
 203         // 衝突点の更新\r
 204         // 両衝突点間の距離が閾値（CONTACT_THRESHOLD）を超えたら消去\r
 205         for(int i=0;i<(int)m_numContacts;i++) {\r
 206                 if(m_contactPoints[i].m_duration > 0) {\r
 207                         PfxVector3 normal = pfxReadVector3(m_contactPoints[i].m_constraintRow[0].m_normal);\r
 208                         PfxVector3 cpA = pA + rotate(qA,pfxReadVector3(m_contactPoints[i].m_localPointA));\r
 209                         PfxVector3 cpB = pB + rotate(qB,pfxReadVector3(m_contactPoints[i].m_localPointB));\r
 210 \r
 211                         // 貫通深度がプラスに転じたかどうかをチェック\r
 212                         PfxFloat distance = dot(normal,(cpA - cpB));\r
 213                         if(distance > SCE_PFX_CONTACT_THRESHOLD_NORMAL) {\r
 214                                 removeContactPoint(i);\r
 215                                 i--;\r
 216                                 continue;\r
 217                         }\r
 218                         m_contactPoints[i].m_distance = distance;\r
 219 \r
 220                         // 深度方向を除去して両点の距離をチェック\r
 221                         cpA = cpA - m_contactPoints[i].m_distance * normal;\r
 222                         PfxFloat distanceAB = lengthSqr(cpA - cpB);\r
 223                         if(distanceAB > SCE_PFX_CONTACT_THRESHOLD_TANGENT) {\r
 224                                 removeContactPoint(i);\r
 225                                 i--;\r
 226                                 continue;\r
 227                         }\r
 228                 }\r
 229                 if(m_contactPoints[i].m_duration < 255) m_contactPoints[i].m_duration++;\r
 230         }\r
 231         if(m_numContacts > 0 && m_duration < 65535) m_duration++;\r
 232 }\r
 233 } //namespace PhysicsEffects\r
 234 } //namespace sce\r