diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/shared')
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; } - } |