summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3Dynamics/shared
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/shared')
-rw-r--r--thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h19
-rw-r--r--thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h133
-rw-r--r--thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h3
-rw-r--r--thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h53
4 files changed, 96 insertions, 112 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h
index 68cf65e312..cf2eed0e7c 100644
--- a/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h
+++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h
@@ -5,30 +5,27 @@
typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
-
struct b3ContactConstraint4
{
-
- b3Float4 m_linear;//normal?
+ b3Float4 m_linear; //normal?
b3Float4 m_worldPos[4];
- b3Float4 m_center; // friction
+ b3Float4 m_center; // friction
float m_jacCoeffInv[4];
float m_b[4];
float m_appliedRambdaDt[4];
- float m_fJacCoeffInv[2]; // friction
- float m_fAppliedRambdaDt[2]; // friction
+ float m_fJacCoeffInv[2]; // friction
+ float m_fAppliedRambdaDt[2]; // friction
unsigned int m_bodyA;
unsigned int m_bodyB;
- int m_batchIdx;
+ int m_batchIdx;
unsigned int m_paddings;
-
};
//inline void setFrictionCoeff(float value) { m_linear[3] = value; }
-inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
+inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint)
{
- return constraint->m_linear.w;
+ return constraint->m_linear.w;
}
-#endif //B3_CONTACT_CONSTRAINT5_H
+#endif //B3_CONTACT_CONSTRAINT5_H
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h
index 805a2bd3ea..3e72f1c3f2 100644
--- a/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h
+++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h
@@ -4,89 +4,84 @@
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-
-void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);
- void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q)
+void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
+void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
{
- if (b3Fabs(n.z) > 0.70710678f) {
- // choose p in y-z plane
- float a = n.y*n.y + n.z*n.z;
- float k = 1.f/sqrt(a);
- p[0].x = 0;
- p[0].y = -n.z*k;
- p[0].z = n.y*k;
- // set q = n x p
- q[0].x = a*k;
- q[0].y = -n.x*p[0].z;
- q[0].z = n.x*p[0].y;
- }
- else {
- // choose p in x-y plane
- float a = n.x*n.x + n.y*n.y;
- float k = 1.f/sqrt(a);
- p[0].x = -n.y*k;
- p[0].y = n.x*k;
- p[0].z = 0;
- // set q = n x p
- q[0].x = -n.z*p[0].y;
- q[0].y = n.z*p[0].x;
- q[0].z = a*k;
- }
+ if (b3Fabs(n.z) > 0.70710678f)
+ {
+ // choose p in y-z plane
+ float a = n.y * n.y + n.z * n.z;
+ float k = 1.f / sqrt(a);
+ p[0].x = 0;
+ p[0].y = -n.z * k;
+ p[0].z = n.y * k;
+ // set q = n x p
+ q[0].x = a * k;
+ q[0].y = -n.x * p[0].z;
+ q[0].z = n.x * p[0].y;
+ }
+ else
+ {
+ // choose p in x-y plane
+ float a = n.x * n.x + n.y * n.y;
+ float k = 1.f / sqrt(a);
+ p[0].x = -n.y * k;
+ p[0].y = n.x * k;
+ p[0].z = 0;
+ // set q = n x p
+ q[0].x = -n.z * p[0].y;
+ q[0].y = n.z * p[0].x;
+ q[0].z = a * k;
+ }
}
-
-
-void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
+void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
{
- *linear = b3MakeFloat4(n.x,n.y,n.z,0.f);
+ *linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
*angular0 = b3Cross3(r0, n);
*angular1 = -b3Cross3(r1, n);
}
-
-float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
- b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 )
+float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
+ b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
{
return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
}
-
float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
- float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
+ float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
{
// linear0,1 are normlized
- float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0;
- float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0);
- float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1;
- float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1);
- return -1.f/(jmj0+jmj1+jmj2+jmj3);
+ float jmj0 = invMass0; //b3Dot3F4(linear0, linear0)*invMass0;
+ float jmj1 = b3Dot3F4(mtMul3(angular0, *invInertia0), angular0);
+ float jmj2 = invMass1; //b3Dot3F4(linear1, linear1)*invMass1;
+ float jmj3 = b3Dot3F4(mtMul3(angular1, *invInertia1), angular1);
+ return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
}
-
-void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
- b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
- __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
- b3ContactConstraint4_t* dstC )
+void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
+ b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
+ __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
+ b3ContactConstraint4_t* dstC)
{
dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
- float dtInv = 1.f/dt;
- for(int ic=0; ic<4; ic++)
+ float dtInv = 1.f / dt;
+ for (int ic = 0; ic < 4; ic++)
{
dstC->m_appliedRambdaDt[ic] = 0.f;
}
dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
-
dstC->m_linear = src->m_worldNormalOnB;
- dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
- for(int ic=0; ic<4; ic++)
+ dstC->m_linear.w = 0.7f; //src->getFrictionCoeff() );
+ for (int ic = 0; ic < 4; ic++)
{
b3Float4 r0 = src->m_worldPosB[ic] - posA;
b3Float4 r1 = src->m_worldPosB[ic] - posB;
- if( ic >= src->m_worldNormalOnB.w )//npoints
+ if (ic >= src->m_worldNormalOnB.w) //npoints
{
dstC->m_jacCoeffInv[ic] = 0.f;
continue;
@@ -98,56 +93,56 @@ void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4Co
setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
- invMassA, &invInertiaA, invMassB, &invInertiaB );
+ invMassA, &invInertiaA, invMassB, &invInertiaB);
relVelN = calcRelVel(linear, -linear, angular0, angular1,
- linVelA, angVelA, linVelB, angVelB);
+ linVelA, angVelA, linVelB, angVelB);
- float e = 0.f;//src->getRestituitionCoeff();
- if( relVelN*relVelN < 0.004f ) e = 0.f;
+ float e = 0.f; //src->getRestituitionCoeff();
+ if (relVelN * relVelN < 0.004f) e = 0.f;
- dstC->m_b[ic] = e*relVelN;
+ dstC->m_b[ic] = e * relVelN;
//float penetration = src->m_worldPosB[ic].w;
- dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
+ dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift) * positionConstraintCoeff * dtInv;
dstC->m_appliedRambdaDt[ic] = 0.f;
}
}
- if( src->m_worldNormalOnB.w > 0 )//npoints
- { // prepare friction
- b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f);
- for(int i=0; i<src->m_worldNormalOnB.w; i++)
+ if (src->m_worldNormalOnB.w > 0) //npoints
+ { // prepare friction
+ b3Float4 center = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
+ for (int i = 0; i < src->m_worldNormalOnB.w; i++)
center += src->m_worldPosB[i];
center /= (float)src->m_worldNormalOnB.w;
b3Float4 tangent[2];
- b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);
-
+ b3PlaneSpace1(src->m_worldNormalOnB, &tangent[0], &tangent[1]);
+
b3Float4 r[2];
r[0] = center - posA;
r[1] = center - posB;
- for(int i=0; i<2; i++)
+ for (int i = 0; i < 2; i++)
{
b3Float4 linear, angular0, angular1;
setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
- invMassA, &invInertiaA, invMassB, &invInertiaB );
+ invMassA, &invInertiaA, invMassB, &invInertiaB);
dstC->m_fAppliedRambdaDt[i] = 0.f;
}
dstC->m_center = center;
}
- for(int i=0; i<4; i++)
+ for (int i = 0; i < 4; i++)
{
- if( i<src->m_worldNormalOnB.w )
+ if (i < src->m_worldNormalOnB.w)
{
dstC->m_worldPos[i] = src->m_worldPosB[i];
}
else
{
- dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f);
+ dstC->m_worldPos[i] = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
}
}
}
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h
index 96fe9f8b39..602a1335aa 100644
--- a/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h
+++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h
@@ -11,5 +11,4 @@ struct b3Inertia
b3Mat3x3 m_initInvInertia;
};
-
-#endif //B3_INERTIA_H \ No newline at end of file
+#endif //B3_INERTIA_H \ No newline at end of file
diff --git a/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h b/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h
index e96f90d3f3..56d9118f95 100644
--- a/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h
+++ b/thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h
@@ -2,11 +2,8 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
-
-
-inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
-
if (bodies[nodeID].m_invMass != 0.f)
{
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
@@ -18,27 +15,27 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
bodies[nodeID].m_angVel.x *= angularDamping;
bodies[nodeID].m_angVel.y *= angularDamping;
bodies[nodeID].m_angVel.z *= angularDamping;
-
+
b3Float4 angvel = bodies[nodeID].m_angVel;
float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
-
+
//limit the angular motion
- if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+ if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
{
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
}
- if(fAngle < 0.001f)
+ if (fAngle < 0.001f)
{
// use Taylor's expansions of sync function
- axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
+ axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
- axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
+ axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
}
-
+
b3Quat dorn;
dorn.x = axis.x;
dorn.y = axis.y;
@@ -47,23 +44,21 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
b3Quat orn0 = bodies[nodeID].m_quat;
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
predictedOrn = b3QuatNormalized(predictedOrn);
- bodies[nodeID].m_quat=predictedOrn;
+ bodies[nodeID].m_quat = predictedOrn;
}
- //linear velocity
- bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
-
+ //linear velocity
+ bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;
+
//apply gravity
bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
-
}
-
}
-inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
-
- if( (body->m_invMass != 0.f))
+
+ if ((body->m_invMass != 0.f))
{
//angular velocity
{
@@ -72,23 +67,23 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
body->m_angVel.x *= angularDamping;
body->m_angVel.y *= angularDamping;
body->m_angVel.z *= angularDamping;
-
+
b3Float4 angvel = body->m_angVel;
float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));
//limit the angular motion
- if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+ if (fAngle * timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
{
fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
}
- if(fAngle < 0.001f)
+ if (fAngle < 0.001f)
{
// use Taylor's expansions of sync function
- axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
+ axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * 0.020833333333f * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
- axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
+ axis = angvel * (b3Sin(0.5f * fAngle * timeStep) / fAngle);
}
b3Quat dorn;
dorn.x = axis.x;
@@ -99,15 +94,13 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
b3Quat predictedOrn = b3QuatMul(dorn, orn0);
predictedOrn = b3QuatNormalized(predictedOrn);
- body->m_quat=predictedOrn;
+ body->m_quat = predictedOrn;
}
//apply gravity
body->m_linVel += gravityAcceleration * timeStep;
- //linear velocity
- body->m_pos += body->m_linVel * timeStep;
-
+ //linear velocity
+ body->m_pos += body->m_linVel * timeStep;
}
-
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-16 11:33:11 +0000