mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +00:00
added jointSolver.cl/h (not working yet)
This commit is contained in:
erwin coumans
parent
b18e03d737
commit
3ca4d68f3e
161
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl
Normal file
161
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl
Normal file
@ -0,0 +1,161 @@
|
||||
|
||||
typedef float4 Quaternion;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float4 m_row[3];
|
||||
}Matrix3x3;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
Matrix3x3 m_invInertia;
|
||||
Matrix3x3 m_initInvInertia;
|
||||
} Shape;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
Matrix3x3 m_basis;//orientation
|
||||
float4 m_origin;//transform
|
||||
}b3Transform;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
b3Transform m_worldTransform;
|
||||
float4 m_deltaLinearVelocity;
|
||||
float4 m_deltaAngularVelocity;
|
||||
float4 m_angularFactor;
|
||||
float4 m_linearFactor;
|
||||
float4 m_invMass;
|
||||
float4 m_pushVelocity;
|
||||
float4 m_turnVelocity;
|
||||
float4 m_linearVelocity;
|
||||
float4 m_angularVelocity;
|
||||
|
||||
union
|
||||
{
|
||||
void* m_originalBody;
|
||||
int m_originalBodyIndex;
|
||||
};
|
||||
} b3SolverBody;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
||||
float4 m_relpos1CrossNormal;
|
||||
float4 m_contactNormal;
|
||||
|
||||
float4 m_relpos2CrossNormal;
|
||||
//float4 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
|
||||
|
||||
float4 m_angularComponentA;
|
||||
float4 m_angularComponentB;
|
||||
|
||||
float4 m_appliedPushImpulse;
|
||||
float4 m_appliedImpulse;
|
||||
|
||||
float m_friction;
|
||||
float m_jacDiagABInv;
|
||||
float m_rhs;
|
||||
float m_cfm;
|
||||
|
||||
float m_lowerLimit;
|
||||
float m_upperLimit;
|
||||
float m_rhsPenetration;
|
||||
|
||||
union
|
||||
{
|
||||
void* m_originalContactPoint;
|
||||
float m_unusedPadding4;
|
||||
};
|
||||
|
||||
int m_overrideNumSolverIterations;
|
||||
int m_frictionIndex;
|
||||
int m_solverBodyIdA;
|
||||
int m_solverBodyIdB;
|
||||
|
||||
} b3SolverConstraint;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int m_bodyAPtrAndSignBit;
|
||||
int m_bodyBPtrAndSignBit;
|
||||
int m_constraintRowOffset;
|
||||
short int m_numConstraintRows;
|
||||
short int m_batchId;
|
||||
|
||||
} b3BatchConstraint;
|
||||
|
||||
#define mymake_float4 (float4)
|
||||
|
||||
|
||||
__inline float dot3F4(float4 a, float4 b)
|
||||
{
|
||||
float4 a1 = mymake_float4(a.xyz,0.f);
|
||||
float4 b1 = mymake_float4(b.xyz,0.f);
|
||||
return dot(a1, b1);
|
||||
}
|
||||
|
||||
__inline void internalApplyImpulse(__global b3SolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude)
|
||||
{
|
||||
body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor;
|
||||
body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor);
|
||||
}
|
||||
|
||||
|
||||
void resolveSingleConstraintRowGeneric(__global b3SolverBody* body1, __global b3SolverBody* body2, __global b3SolverConstraint* c)
|
||||
{
|
||||
float deltaImpulse = c->m_rhs-c->m_appliedImpulse.x*c->m_cfm;
|
||||
float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity);
|
||||
float deltaVel2Dotn = -dot3F4(c->m_contactNormal,body2->m_deltaLinearVelocity) + dot3F4(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity);
|
||||
|
||||
deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv;
|
||||
deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv;
|
||||
|
||||
float sum = c->m_appliedImpulse.x + deltaImpulse;
|
||||
if (sum < c->m_lowerLimit)
|
||||
{
|
||||
deltaImpulse = c->m_lowerLimit-c->m_appliedImpulse.x;
|
||||
c->m_appliedImpulse.x = c->m_lowerLimit;
|
||||
}
|
||||
else if (sum > c->m_upperLimit)
|
||||
{
|
||||
deltaImpulse = c->m_upperLimit-c->m_appliedImpulse.x;
|
||||
c->m_appliedImpulse.x = c->m_upperLimit;
|
||||
}
|
||||
else
|
||||
{
|
||||
c->m_appliedImpulse.x = sum;
|
||||
}
|
||||
|
||||
if (body1->m_invMass.x)
|
||||
internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse);
|
||||
|
||||
if (body2->m_invMass.x)
|
||||
internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse);
|
||||
|
||||
}
|
||||
|
||||
__kernel
|
||||
void solveJointConstraintRows(__global b3SolverBody* solverBodies,
|
||||
__global b3BatchConstraint* batchConstraints,
|
||||
__global b3SolverConstraint* rows,
|
||||
int batchOffset,
|
||||
int constraintOffset,
|
||||
int numConstraintsInBatch
|
||||
)
|
||||
{
|
||||
int b = get_global_id(0);
|
||||
if (b>=numConstraintsInBatch)
|
||||
return;
|
||||
|
||||
__global b3BatchConstraint* c = &batchConstraints[b+batchOffset];
|
||||
for (int jj=0;jj<c->m_numConstraintRows;jj++)
|
||||
{
|
||||
__global b3SolverConstraint* constraint = &rows[c->m_constraintRowOffset+jj];
|
||||
resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);
|
||||
}
|
||||
};
|
||||
164
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h
Normal file
164
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h
Normal file
@ -0,0 +1,164 @@
|
||||
//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
|
||||
static const char* solveConstraintRowsCL= \
|
||||
"\n"
|
||||
"typedef float4 Quaternion;\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" float4 m_row[3];\n"
|
||||
"}Matrix3x3;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" Matrix3x3 m_invInertia;\n"
|
||||
" Matrix3x3 m_initInvInertia;\n"
|
||||
"} Shape;\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" Matrix3x3 m_basis;//orientation\n"
|
||||
" float4 m_origin;//transform\n"
|
||||
"}b3Transform;\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" b3Transform m_worldTransform;\n"
|
||||
" float4 m_deltaLinearVelocity;\n"
|
||||
" float4 m_deltaAngularVelocity;\n"
|
||||
" float4 m_angularFactor;\n"
|
||||
" float4 m_linearFactor;\n"
|
||||
" float4 m_invMass;\n"
|
||||
" float4 m_pushVelocity;\n"
|
||||
" float4 m_turnVelocity;\n"
|
||||
" float4 m_linearVelocity;\n"
|
||||
" float4 m_angularVelocity;\n"
|
||||
"\n"
|
||||
" union \n"
|
||||
" {\n"
|
||||
" void* m_originalBody;\n"
|
||||
" int m_originalBodyIndex;\n"
|
||||
" };\n"
|
||||
"} b3SolverBody;\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
"\n"
|
||||
" float4 m_relpos1CrossNormal;\n"
|
||||
" float4 m_contactNormal;\n"
|
||||
"\n"
|
||||
" float4 m_relpos2CrossNormal;\n"
|
||||
" //float4 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal\n"
|
||||
"\n"
|
||||
" float4 m_angularComponentA;\n"
|
||||
" float4 m_angularComponentB;\n"
|
||||
" \n"
|
||||
" float4 m_appliedPushImpulse;\n"
|
||||
" float4 m_appliedImpulse;\n"
|
||||
"\n"
|
||||
" float m_friction;\n"
|
||||
" float m_jacDiagABInv;\n"
|
||||
" float m_rhs;\n"
|
||||
" float m_cfm;\n"
|
||||
" \n"
|
||||
" float m_lowerLimit;\n"
|
||||
" float m_upperLimit;\n"
|
||||
" float m_rhsPenetration;\n"
|
||||
"\n"
|
||||
" union\n"
|
||||
" {\n"
|
||||
" void* m_originalContactPoint;\n"
|
||||
" float m_unusedPadding4;\n"
|
||||
" };\n"
|
||||
"\n"
|
||||
" int m_overrideNumSolverIterations;\n"
|
||||
" int m_frictionIndex;\n"
|
||||
" int m_solverBodyIdA;\n"
|
||||
" int m_solverBodyIdB;\n"
|
||||
"\n"
|
||||
"} b3SolverConstraint;\n"
|
||||
"\n"
|
||||
"typedef struct\n"
|
||||
"{\n"
|
||||
" int m_bodyAPtrAndSignBit;\n"
|
||||
" int m_bodyBPtrAndSignBit;\n"
|
||||
" int m_constraintRowOffset;\n"
|
||||
" short int m_numConstraintRows;\n"
|
||||
" short int m_batchId;\n"
|
||||
"\n"
|
||||
"} b3BatchConstraint;\n"
|
||||
"\n"
|
||||
"#define mymake_float4 (float4)\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"__inline float dot3F4(float4 a, float4 b)\n"
|
||||
"{\n"
|
||||
" float4 a1 = mymake_float4(a.xyz,0.f);\n"
|
||||
" float4 b1 = mymake_float4(b.xyz,0.f);\n"
|
||||
" return dot(a1, b1);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__inline void internalApplyImpulse(__global b3SolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude)\n"
|
||||
"{\n"
|
||||
" body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor;\n"
|
||||
" body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor);\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"\n"
|
||||
"void resolveSingleConstraintRowGeneric(__global b3SolverBody* body1, __global b3SolverBody* body2, __global b3SolverConstraint* c)\n"
|
||||
"{\n"
|
||||
" float deltaImpulse = c->m_rhs-c->m_appliedImpulse.x*c->m_cfm;\n"
|
||||
" float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity);\n"
|
||||
" float deltaVel2Dotn = -dot3F4(c->m_contactNormal,body2->m_deltaLinearVelocity) + dot3F4(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity);\n"
|
||||
"\n"
|
||||
" deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv;\n"
|
||||
" deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv;\n"
|
||||
"\n"
|
||||
" float sum = c->m_appliedImpulse.x + deltaImpulse;\n"
|
||||
" if (sum < c->m_lowerLimit)\n"
|
||||
" {\n"
|
||||
" deltaImpulse = c->m_lowerLimit-c->m_appliedImpulse.x;\n"
|
||||
" c->m_appliedImpulse.x = c->m_lowerLimit;\n"
|
||||
" }\n"
|
||||
" else if (sum > c->m_upperLimit) \n"
|
||||
" {\n"
|
||||
" deltaImpulse = c->m_upperLimit-c->m_appliedImpulse.x;\n"
|
||||
" c->m_appliedImpulse.x = c->m_upperLimit;\n"
|
||||
" }\n"
|
||||
" else\n"
|
||||
" {\n"
|
||||
" c->m_appliedImpulse.x = sum;\n"
|
||||
" }\n"
|
||||
"\n"
|
||||
" if (body1->m_invMass.x)\n"
|
||||
" internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse);\n"
|
||||
" \n"
|
||||
" if (body2->m_invMass.x)\n"
|
||||
" internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse);\n"
|
||||
"\n"
|
||||
"}\n"
|
||||
"\n"
|
||||
"__kernel\n"
|
||||
"void solveJointConstraintRows(__global b3SolverBody* solverBodies,\n"
|
||||
" __global b3BatchConstraint* batchConstraints,\n"
|
||||
" __global b3SolverConstraint* rows,\n"
|
||||
" int batchOffset,\n"
|
||||
" int constraintOffset,\n"
|
||||
" int numConstraintsInBatch\n"
|
||||
" )\n"
|
||||
"{\n"
|
||||
" int b = get_global_id(0);\n"
|
||||
" if (b>=numConstraintsInBatch)\n"
|
||||
" return;\n"
|
||||
"\n"
|
||||
" __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];\n"
|
||||
" for (int jj=0;jj<c->m_numConstraintRows;jj++)\n"
|
||||
" {\n"
|
||||
" __global b3SolverConstraint* constraint = &rows[c->m_constraintRowOffset+jj];\n"
|
||||
" resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);\n"
|
||||
" }\n"
|
||||
"};\n"
|
||||
;
|
||||
Loading…
Reference in New Issue
Block a user