summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h')
-rw-r--r--thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h143
1 files changed, 67 insertions, 76 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
index a3fa82287b..acf7c1b180 100644
--- a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
+++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
@@ -2,32 +2,24 @@
#ifndef B3_CONTACT_SPHERE_SPHERE_H
#define B3_CONTACT_SPHERE_SPHERE_H
-
-
-
-
-void computeContactSphereConvex(int pairIndex,
- int bodyIndexA, int bodyIndexB,
- int collidableIndexA, int collidableIndexB,
- const b3RigidBodyData* rigidBodies,
- const b3Collidable* collidables,
- const b3ConvexPolyhedronData* convexShapes,
- const b3Vector3* convexVertices,
- const int* convexIndices,
- const b3GpuFace* faces,
- b3Contact4* globalContactsOut,
- int& nGlobalContactsOut,
- int maxContactCapacity)
+void computeContactSphereConvex(int pairIndex,
+ int bodyIndexA, int bodyIndexB,
+ int collidableIndexA, int collidableIndexB,
+ const b3RigidBodyData* rigidBodies,
+ const b3Collidable* collidables,
+ const b3ConvexPolyhedronData* convexShapes,
+ const b3Vector3* convexVertices,
+ const int* convexIndices,
+ const b3GpuFace* faces,
+ b3Contact4* globalContactsOut,
+ int& nGlobalContactsOut,
+ int maxContactCapacity)
{
-
float radius = collidables[collidableIndexA].m_radius;
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
-
-
float4 pos = rigidBodies[bodyIndexB].m_pos;
-
b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
@@ -44,63 +36,64 @@ void computeContactSphereConvex(int pairIndex,
int numFaces = convexShapes[shapeIndex].m_numFaces;
float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
- float minDist = -1000000.f; // TODO: What is the largest/smallest float?
+ float minDist = -1000000.f; // TODO: What is the largest/smallest float?
bool bCollide = true;
int region = -1;
float4 localHitNormal;
- for ( int f = 0; f < numFaces; f++ )
+ for (int f = 0; f < numFaces; f++)
{
- b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
+ b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
float4 planeEqn;
- float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
- float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
+ float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
+ float4 n1 = localPlaneNormal; //quatRotate(quat,localPlaneNormal);
planeEqn = n1;
planeEqn[3] = face.m_plane.w;
float4 pntReturn;
float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
- if ( dist > radius)
+ if (dist > radius)
{
bCollide = false;
break;
}
- if ( dist > 0 )
+ if (dist > 0)
{
//might hit an edge or vertex
b3Vector3 out;
bool isInPoly = IsPointInPolygon(spherePos,
- &face,
- &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
- convexIndices,
- &out);
+ &face,
+ &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
+ convexIndices,
+ &out);
if (isInPoly)
{
- if (dist>minDist)
+ if (dist > minDist)
{
minDist = dist;
closestPnt = pntReturn;
localHitNormal = planeEqn;
- region=1;
+ region = 1;
}
- } else
+ }
+ else
{
- b3Vector3 tmp = spherePos-out;
+ b3Vector3 tmp = spherePos - out;
b3Scalar l2 = tmp.length2();
- if (l2<radius*radius)
+ if (l2 < radius * radius)
{
- dist = b3Sqrt(l2);
- if (dist>minDist)
+ dist = b3Sqrt(l2);
+ if (dist > minDist)
{
minDist = dist;
closestPnt = out;
- localHitNormal = tmp/dist;
- region=2;
+ localHitNormal = tmp / dist;
+ region = 2;
}
-
- } else
+ }
+ else
{
bCollide = false;
break;
@@ -109,12 +102,12 @@ void computeContactSphereConvex(int pairIndex,
}
else
{
- if ( dist > minDist )
+ if (dist > minDist)
{
minDist = dist;
closestPnt = pntReturn;
localHitNormal = planeEqn;
- region=3;
+ region = 3;
}
}
}
@@ -123,40 +116,38 @@ void computeContactSphereConvex(int pairIndex,
if (bCollide && minDist > -10000)
{
-
- float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
+ float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal; //-hitNormalWorld;
float4 pOnB1 = tr(closestPnt);
//printf("dist ,%f,",minDist);
- float actualDepth = minDist-radius;
- if (actualDepth<0)
- {
- //printf("actualDepth = ,%f,", actualDepth);
- //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
- //printf("region=,%d,\n", region);
- pOnB1[3] = actualDepth;
-
- int dstIdx;
-// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
-
- if (nGlobalContactsOut < maxContactCapacity)
+ float actualDepth = minDist - radius;
+ if (actualDepth < 0)
{
- dstIdx=nGlobalContactsOut;
- nGlobalContactsOut++;
-
- b3Contact4* c = &globalContactsOut[dstIdx];
- c->m_worldNormalOnB = normalOnSurfaceB1;
- c->setFrictionCoeff(0.7);
- c->setRestituitionCoeff(0.f);
-
- c->m_batchIdx = pairIndex;
- c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
- c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
- c->m_worldPosB[0] = pOnB1;
- int numPoints = 1;
- c->m_worldNormalOnB.w = (b3Scalar)numPoints;
- }//if (dstIdx < numPairs)
+ //printf("actualDepth = ,%f,", actualDepth);
+ //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
+ //printf("region=,%d,\n", region);
+ pOnB1[3] = actualDepth;
+
+ int dstIdx;
+ // dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
+
+ if (nGlobalContactsOut < maxContactCapacity)
+ {
+ dstIdx = nGlobalContactsOut;
+ nGlobalContactsOut++;
+
+ b3Contact4* c = &globalContactsOut[dstIdx];
+ c->m_worldNormalOnB = normalOnSurfaceB1;
+ c->setFrictionCoeff(0.7);
+ c->setRestituitionCoeff(0.f);
+
+ c->m_batchIdx = pairIndex;
+ c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
+ c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
+ c->m_worldPosB[0] = pOnB1;
+ int numPoints = 1;
+ c->m_worldNormalOnB.w = (b3Scalar)numPoints;
+ } //if (dstIdx < numPairs)
}
- }//if (hasCollision)
-
+ } //if (hasCollision)
}
-#endif //B3_CONTACT_SPHERE_SPHERE_H
+#endif //B3_CONTACT_SPHERE_SPHERE_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-08 00:40:38 +0000