diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3OpenCL/RigidBody')
29 files changed, 7639 insertions, 8305 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h index c7478f54a1..89c0142ab3 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h @@ -5,14 +5,13 @@ #include "Bullet3Dynamics/shared/b3ContactConstraint4.h" - -B3_ATTRIBUTE_ALIGNED16(struct) b3GpuConstraint4 : public b3ContactConstraint4 +B3_ATTRIBUTE_ALIGNED16(struct) +b3GpuConstraint4 : public b3ContactConstraint4 { B3_DECLARE_ALIGNED_ALLOCATOR(); - inline void setFrictionCoeff(float value) { m_linear[3] = value; } - inline float getFrictionCoeff() const { return m_linear[3]; } + inline void setFrictionCoeff(float value) { m_linear[3] = value; } + inline float getFrictionCoeff() const { return m_linear[3]; } }; -#endif //B3_CONSTRAINT4_h - +#endif //B3_CONSTRAINT4_h diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp index af687b54e9..a271090af4 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp @@ -19,11 +19,11 @@ subject to the following restrictions: #include <new> #include "Bullet3Common/b3Transform.h" -void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyData* bodies) +void b3GpuGenericConstraint::getInfo1(unsigned int* info, const b3RigidBodyData* bodies) { switch (m_constraintType) { - case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: { *info = 3; break; @@ -35,7 +35,7 @@ void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyData* }; } -void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) +void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) { b3Transform trA; trA.setIdentity(); @@ -47,54 +47,52 @@ void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo trB.setOrigin(bodies[constraint->m_rbB].m_pos); trB.setRotation(bodies[constraint->m_rbB].m_quat); - // anchor points in global coordinates with respect to body PORs. - - // set jacobian - info->m_J1linearAxis[0] = 1; - info->m_J1linearAxis[info->rowskip+1] = 1; - info->m_J1linearAxis[2*info->rowskip+2] = 1; + // anchor points in global coordinates with respect to body PORs. - b3Vector3 a1 = trA.getBasis()*constraint->getPivotInA(); + // set jacobian + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip + 1] = 1; + info->m_J1linearAxis[2 * info->rowskip + 2] = 1; + + b3Vector3 a1 = trA.getBasis() * constraint->getPivotInA(); //b3Vector3 a1a = b3QuatRotate(trA.getRotation(),constraint->getPivotInA()); { b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); + b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis + info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis + 2 * info->rowskip); b3Vector3 a1neg = -a1; - a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2); } - + if (info->m_J2linearAxis) { info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[info->rowskip+1] = -1; - info->m_J2linearAxis[2*info->rowskip+2] = -1; + info->m_J2linearAxis[info->rowskip + 1] = -1; + info->m_J2linearAxis[2 * info->rowskip + 2] = -1; } - - b3Vector3 a2 = trB.getBasis()*constraint->getPivotInB(); - + + b3Vector3 a2 = trB.getBasis() * constraint->getPivotInB(); + { - // b3Vector3 a2n = -a2; + // b3Vector3 a2n = -a2; b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); - b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); - b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); - a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis + info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis + 2 * info->rowskip); + a2.getSkewSymmetricMatrix(angular0, angular1, angular2); } - - - // set right hand side -// b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; + // set right hand side + // b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; b3Scalar currERP = info->erp; b3Scalar k = info->fps * currERP; - int j; - for (j=0; j<3; j++) - { - info->m_constraintError[j*info->rowskip] = k * (a2[j] + trB.getOrigin()[j] - a1[j] - trA.getOrigin()[j]); + int j; + for (j = 0; j < 3; j++) + { + info->m_constraintError[j * info->rowskip] = k * (a2[j] + trB.getOrigin()[j] - a1[j] - trA.getOrigin()[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); - } + } #if 0 if(m_flags & B3_P2P_FLAGS_CFM) { @@ -117,21 +115,20 @@ void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo } info->m_damping = m_setting.m_damping; #endif - } -void b3GpuGenericConstraint::getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) +void b3GpuGenericConstraint::getInfo2(b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) { switch (m_constraintType) { - case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: { - getInfo2Point2Point(this,info,bodies); + getInfo2Point2Point(this, info, bodies); break; }; default: - { - b3Assert(0); - } + { + b3Assert(0); + } }; } diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h index 14b3ba7fec..1f163ba7d5 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h @@ -20,37 +20,35 @@ subject to the following restrictions: struct b3RigidBodyData; enum B3_CONSTRAINT_FLAGS { - B3_CONSTRAINT_FLAG_ENABLED=1, + B3_CONSTRAINT_FLAG_ENABLED = 1, }; enum b3GpuGenericConstraintType { - B3_GPU_POINT2POINT_CONSTRAINT_TYPE=3, - B3_GPU_FIXED_CONSTRAINT_TYPE=4, -// B3_HINGE_CONSTRAINT_TYPE, -// B3_CONETWIST_CONSTRAINT_TYPE, -// B3_D6_CONSTRAINT_TYPE, -// B3_SLIDER_CONSTRAINT_TYPE, -// B3_CONTACT_CONSTRAINT_TYPE, -// B3_D6_SPRING_CONSTRAINT_TYPE, -// B3_GEAR_CONSTRAINT_TYPE, - + B3_GPU_POINT2POINT_CONSTRAINT_TYPE = 3, + B3_GPU_FIXED_CONSTRAINT_TYPE = 4, + // B3_HINGE_CONSTRAINT_TYPE, + // B3_CONETWIST_CONSTRAINT_TYPE, + // B3_D6_CONSTRAINT_TYPE, + // B3_SLIDER_CONSTRAINT_TYPE, + // B3_CONTACT_CONSTRAINT_TYPE, + // B3_D6_SPRING_CONSTRAINT_TYPE, + // B3_GEAR_CONSTRAINT_TYPE, + B3_GPU_MAX_CONSTRAINT_TYPE }; - - -struct b3GpuConstraintInfo2 +struct b3GpuConstraintInfo2 { // integrator parameters: frames per second (1/stepsize), default error // reduction parameter (0..1). - b3Scalar fps,erp; + b3Scalar fps, erp; // for the first and second body, pointers to two (linear and angular) // n*3 jacobian sub matrices, stored by rows. these matrices will have // been initialized to 0 on entry. if the second body is zero then the // J2xx pointers may be 0. - b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis; + b3Scalar *m_J1linearAxis, *m_J1angularAxis, *m_J2linearAxis, *m_J2angularAxis; // elements to jump from one row to the next in J's int rowskip; @@ -58,44 +56,44 @@ struct b3GpuConstraintInfo2 // right hand sides of the equation J*v = c + cfm * lambda. cfm is the // "constraint force mixing" vector. c is set to zero on entry, cfm is // set to a constant value (typically very small or zero) value on entry. - b3Scalar *m_constraintError,*cfm; + b3Scalar *m_constraintError, *cfm; // lo and hi limits for variables (set to -/+ infinity on entry). - b3Scalar *m_lowerLimit,*m_upperLimit; + b3Scalar *m_lowerLimit, *m_upperLimit; // findex vector for variables. see the LCP solver interface for a // description of what this does. this is set to -1 on entry. // note that the returned indexes are relative to the first index of // the constraint. - int *findex; + int* findex; // number of solver iterations int m_numIterations; //damping of the velocity - b3Scalar m_damping; + b3Scalar m_damping; }; - -B3_ATTRIBUTE_ALIGNED16(struct) b3GpuGenericConstraint +B3_ATTRIBUTE_ALIGNED16(struct) +b3GpuGenericConstraint { - int m_constraintType; - int m_rbA; - int m_rbB; - float m_breakingImpulseThreshold; + int m_constraintType; + int m_rbA; + int m_rbB; + float m_breakingImpulseThreshold; b3Vector3 m_pivotInA; b3Vector3 m_pivotInB; b3Quaternion m_relTargetAB; - int m_flags; + int m_flags; int m_uid; int m_padding[2]; - int getRigidBodyA() const + int getRigidBodyA() const { return m_rbA; } - int getRigidBodyB() const + int getRigidBodyB() const { return m_rbB; } @@ -121,12 +119,10 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3GpuGenericConstraint } ///internal method used by the constraint solver, don't use them directly - void getInfo1 (unsigned int* info,const b3RigidBodyData* bodies); + void getInfo1(unsigned int* info, const b3RigidBodyData* bodies); ///internal method used by the constraint solver, don't use them directly - void getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies); - - + void getInfo2(b3GpuConstraintInfo2 * info, const b3RigidBodyData* bodies); }; -#endif //B3_GPU_GENERIC_CONSTRAINT_H
\ No newline at end of file +#endif //B3_GPU_GENERIC_CONSTRAINT_H
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp index 179dfc4f26..089fb1f6a6 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp @@ -2,7 +2,7 @@ #include "b3GpuJacobiContactSolver.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" #include "Bullet3Common/b3AlignedObjectArray.h" -#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" //b3Int2 +#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" //b3Int2 class b3Vector3; #include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" #include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" @@ -15,89 +15,78 @@ class b3Vector3; #include "Bullet3Common/shared/b3Int4.h" #define SOLVER_UTILS_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl" - struct b3GpuJacobiSolverInternalData { - //btRadixSort32CL* m_sort32; - //btBoundSearchCL* m_search; - b3PrefixScanCL* m_scan; - - b3OpenCLArray<unsigned int>* m_bodyCount; - b3OpenCLArray<b3Int2>* m_contactConstraintOffsets; - b3OpenCLArray<unsigned int>* m_offsetSplitBodies; - - b3OpenCLArray<b3Vector3>* m_deltaLinearVelocities; - b3OpenCLArray<b3Vector3>* m_deltaAngularVelocities; - - b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocitiesCPU; - b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocitiesCPU; + //btRadixSort32CL* m_sort32; + //btBoundSearchCL* m_search; + b3PrefixScanCL* m_scan; + b3OpenCLArray<unsigned int>* m_bodyCount; + b3OpenCLArray<b3Int2>* m_contactConstraintOffsets; + b3OpenCLArray<unsigned int>* m_offsetSplitBodies; + b3OpenCLArray<b3Vector3>* m_deltaLinearVelocities; + b3OpenCLArray<b3Vector3>* m_deltaAngularVelocities; - b3OpenCLArray<b3GpuConstraint4>* m_contactConstraints; - - b3FillCL* m_filler; - - - cl_kernel m_countBodiesKernel; - cl_kernel m_contactToConstraintSplitKernel; - cl_kernel m_clearVelocitiesKernel; - cl_kernel m_averageVelocitiesKernel; - cl_kernel m_updateBodyVelocitiesKernel; - cl_kernel m_solveContactKernel; - cl_kernel m_solveFrictionKernel; + b3AlignedObjectArray<b3Vector3> m_deltaLinearVelocitiesCPU; + b3AlignedObjectArray<b3Vector3> m_deltaAngularVelocitiesCPU; + b3OpenCLArray<b3GpuConstraint4>* m_contactConstraints; + b3FillCL* m_filler; + cl_kernel m_countBodiesKernel; + cl_kernel m_contactToConstraintSplitKernel; + cl_kernel m_clearVelocitiesKernel; + cl_kernel m_averageVelocitiesKernel; + cl_kernel m_updateBodyVelocitiesKernel; + cl_kernel m_solveContactKernel; + cl_kernel m_solveFrictionKernel; }; - b3GpuJacobiContactSolver::b3GpuJacobiContactSolver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity) - :m_context(ctx), - m_device(device), - m_queue(queue) + : m_context(ctx), + m_device(device), + m_queue(queue) { m_data = new b3GpuJacobiSolverInternalData; - m_data->m_scan = new b3PrefixScanCL(m_context,m_device,m_queue); - m_data->m_bodyCount = new b3OpenCLArray<unsigned int>(m_context,m_queue); - m_data->m_filler = new b3FillCL(m_context,m_device,m_queue); - m_data->m_contactConstraintOffsets = new b3OpenCLArray<b3Int2>(m_context,m_queue); - m_data->m_offsetSplitBodies = new b3OpenCLArray<unsigned int>(m_context,m_queue); - m_data->m_contactConstraints = new b3OpenCLArray<b3GpuConstraint4>(m_context,m_queue); - m_data->m_deltaLinearVelocities = new b3OpenCLArray<b3Vector3>(m_context,m_queue); - m_data->m_deltaAngularVelocities = new b3OpenCLArray<b3Vector3>(m_context,m_queue); + m_data->m_scan = new b3PrefixScanCL(m_context, m_device, m_queue); + m_data->m_bodyCount = new b3OpenCLArray<unsigned int>(m_context, m_queue); + m_data->m_filler = new b3FillCL(m_context, m_device, m_queue); + m_data->m_contactConstraintOffsets = new b3OpenCLArray<b3Int2>(m_context, m_queue); + m_data->m_offsetSplitBodies = new b3OpenCLArray<unsigned int>(m_context, m_queue); + m_data->m_contactConstraints = new b3OpenCLArray<b3GpuConstraint4>(m_context, m_queue); + m_data->m_deltaLinearVelocities = new b3OpenCLArray<b3Vector3>(m_context, m_queue); + m_data->m_deltaAngularVelocities = new b3OpenCLArray<b3Vector3>(m_context, m_queue); cl_int pErrNum; - const char* additionalMacros=""; + const char* additionalMacros = ""; const char* solverUtilsSource = solverUtilsCL; { - cl_program solverUtilsProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverUtilsSource, &pErrNum,additionalMacros, SOLVER_UTILS_KERNEL_PATH); + cl_program solverUtilsProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solverUtilsSource, &pErrNum, additionalMacros, SOLVER_UTILS_KERNEL_PATH); b3Assert(solverUtilsProg); - m_data->m_countBodiesKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "CountBodiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + m_data->m_countBodiesKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverUtilsSource, "CountBodiesKernel", &pErrNum, solverUtilsProg, additionalMacros); b3Assert(m_data->m_countBodiesKernel); - m_data->m_contactToConstraintSplitKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "ContactToConstraintSplitKernel", &pErrNum, solverUtilsProg,additionalMacros ); + m_data->m_contactToConstraintSplitKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverUtilsSource, "ContactToConstraintSplitKernel", &pErrNum, solverUtilsProg, additionalMacros); b3Assert(m_data->m_contactToConstraintSplitKernel); - m_data->m_clearVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "ClearVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + m_data->m_clearVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverUtilsSource, "ClearVelocitiesKernel", &pErrNum, solverUtilsProg, additionalMacros); b3Assert(m_data->m_clearVelocitiesKernel); - m_data->m_averageVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "AverageVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + m_data->m_averageVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverUtilsSource, "AverageVelocitiesKernel", &pErrNum, solverUtilsProg, additionalMacros); b3Assert(m_data->m_averageVelocitiesKernel); - m_data->m_updateBodyVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "UpdateBodyVelocitiesKernel", &pErrNum, solverUtilsProg,additionalMacros ); + m_data->m_updateBodyVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverUtilsSource, "UpdateBodyVelocitiesKernel", &pErrNum, solverUtilsProg, additionalMacros); b3Assert(m_data->m_updateBodyVelocitiesKernel); - - m_data->m_solveContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "SolveContactJacobiKernel", &pErrNum, solverUtilsProg,additionalMacros ); - b3Assert(m_data->m_solveContactKernel ); + m_data->m_solveContactKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverUtilsSource, "SolveContactJacobiKernel", &pErrNum, solverUtilsProg, additionalMacros); + b3Assert(m_data->m_solveContactKernel); - m_data->m_solveFrictionKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "SolveFrictionJacobiKernel", &pErrNum, solverUtilsProg,additionalMacros ); + m_data->m_solveFrictionKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverUtilsSource, "SolveFrictionJacobiKernel", &pErrNum, solverUtilsProg, additionalMacros); b3Assert(m_data->m_solveFrictionKernel); } - } - b3GpuJacobiContactSolver::~b3GpuJacobiContactSolver() { clReleaseKernel(m_data->m_solveContactKernel); @@ -106,7 +95,7 @@ b3GpuJacobiContactSolver::~b3GpuJacobiContactSolver() clReleaseKernel(m_data->m_contactToConstraintSplitKernel); clReleaseKernel(m_data->m_averageVelocitiesKernel); clReleaseKernel(m_data->m_updateBodyVelocitiesKernel); - clReleaseKernel(m_data->m_clearVelocitiesKernel ); + clReleaseKernel(m_data->m_clearVelocitiesKernel); delete m_data->m_deltaLinearVelocities; delete m_data->m_deltaAngularVelocities; @@ -119,80 +108,70 @@ b3GpuJacobiContactSolver::~b3GpuJacobiContactSolver() delete m_data; } - - b3Vector3 make_float4(float v) { - return b3MakeVector3 (v,v,v); + return b3MakeVector3(v, v, v); } -b3Vector4 make_float4(float x,float y, float z, float w) +b3Vector4 make_float4(float x, float y, float z, float w) { - return b3MakeVector4 (x,y,z,w); + return b3MakeVector4(x, y, z, w); } - - static - inline - float calcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, - const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1) - { - return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1); - } - - - static - inline - void setLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1, - b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1) - { - linear = n; - angular0 = b3Cross(r0, n); - angular1 = -b3Cross(r1, n); - } - - -static __inline void solveContact(b3GpuConstraint4& cs, - const b3Vector3& posA, const b3Vector3& linVelARO, const b3Vector3& angVelARO, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, const b3Vector3& linVelBRO, const b3Vector3& angVelBRO, float invMassB, const b3Matrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4], b3Vector3& dLinVelA, b3Vector3& dAngVelA, b3Vector3& dLinVelB, b3Vector3& dAngVelB) +static inline float calcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, + const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1) { + return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1); +} +static inline void setLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1, + b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1) +{ + linear = n; + angular0 = b3Cross(r0, n); + angular1 = -b3Cross(r1, n); +} - for(int ic=0; ic<4; ic++) +static __inline void solveContact(b3GpuConstraint4& cs, + const b3Vector3& posA, const b3Vector3& linVelARO, const b3Vector3& angVelARO, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, const b3Vector3& linVelBRO, const b3Vector3& angVelBRO, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4], b3Vector3& dLinVelA, b3Vector3& dAngVelA, b3Vector3& dLinVelB, b3Vector3& dAngVelB) +{ + for (int ic = 0; ic < 4; ic++) { // dont necessary because this makes change to 0 - if( cs.m_jacCoeffInv[ic] == 0.f ) continue; + if (cs.m_jacCoeffInv[ic] == 0.f) continue; { b3Vector3 angular0, angular1, linear; b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA; b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB; - setLinearAndAngular( (const b3Vector3 &)cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 ); + setLinearAndAngular((const b3Vector3&)cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1); - float rambdaDt = calcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1, - linVelARO+dLinVelA, angVelARO+dAngVelA, linVelBRO+dLinVelB, angVelBRO+dAngVelB ) + cs.m_b[ic]; + float rambdaDt = calcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1, + linVelARO + dLinVelA, angVelARO + dAngVelA, linVelBRO + dLinVelB, angVelBRO + dAngVelB) + + cs.m_b[ic]; rambdaDt *= cs.m_jacCoeffInv[ic]; { float prevSum = cs.m_appliedRambdaDt[ic]; float updated = prevSum; updated += rambdaDt; - updated = b3Max( updated, minRambdaDt[ic] ); - updated = b3Min( updated, maxRambdaDt[ic] ); + updated = b3Max(updated, minRambdaDt[ic]); + updated = b3Min(updated, maxRambdaDt[ic]); rambdaDt = updated - prevSum; cs.m_appliedRambdaDt[ic] = updated; } - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; + b3Vector3 linImp0 = invMassA * linear * rambdaDt; + b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt; + b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt; + b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt; #ifdef _WIN32 - b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp0.getX())); b3Assert(_finite(linImp1.getX())); #endif - + if (invMassA) { dLinVelA += linImp0; @@ -207,43 +186,42 @@ static __inline void solveContact(b3GpuConstraint4& cs, } } - - void solveContact3(b3GpuConstraint4* cs, - b3Vector3* posAPtr, b3Vector3* linVelA, b3Vector3* angVelA, float invMassA, const b3Matrix3x3& invInertiaA, - b3Vector3* posBPtr, b3Vector3* linVelB, b3Vector3* angVelB, float invMassB, const b3Matrix3x3& invInertiaB, - b3Vector3* dLinVelA, b3Vector3* dAngVelA, b3Vector3* dLinVelB, b3Vector3* dAngVelB) + b3Vector3* posAPtr, b3Vector3* linVelA, b3Vector3* angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + b3Vector3* posBPtr, b3Vector3* linVelB, b3Vector3* angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + b3Vector3* dLinVelA, b3Vector3* dAngVelA, b3Vector3* dLinVelB, b3Vector3* dAngVelB) { float minRambdaDt = 0; float maxRambdaDt = FLT_MAX; - for(int ic=0; ic<4; ic++) + for (int ic = 0; ic < 4; ic++) { - if( cs->m_jacCoeffInv[ic] == 0.f ) continue; + if (cs->m_jacCoeffInv[ic] == 0.f) continue; b3Vector3 angular0, angular1, linear; b3Vector3 r0 = cs->m_worldPos[ic] - *posAPtr; b3Vector3 r1 = cs->m_worldPos[ic] - *posBPtr; - setLinearAndAngular( cs->m_linear, r0, r1, linear, angular0, angular1 ); + setLinearAndAngular(cs->m_linear, r0, r1, linear, angular0, angular1); - float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, - *linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic]; + float rambdaDt = calcRelVel(cs->m_linear, -cs->m_linear, angular0, angular1, + *linVelA + *dLinVelA, *angVelA + *dAngVelA, *linVelB + *dLinVelB, *angVelB + *dAngVelB) + + cs->m_b[ic]; rambdaDt *= cs->m_jacCoeffInv[ic]; { float prevSum = cs->m_appliedRambdaDt[ic]; float updated = prevSum; updated += rambdaDt; - updated = b3Max( updated, minRambdaDt ); - updated = b3Min( updated, maxRambdaDt ); + updated = b3Max(updated, minRambdaDt); + updated = b3Min(updated, maxRambdaDt); rambdaDt = updated - prevSum; cs->m_appliedRambdaDt[ic] = updated; } - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; + b3Vector3 linImp0 = invMassA * linear * rambdaDt; + b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt; + b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt; + b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt; if (invMassA) { @@ -258,58 +236,56 @@ void solveContact3(b3GpuConstraint4* cs, } } - -static inline void solveFriction(b3GpuConstraint4& cs, - const b3Vector3& posA, const b3Vector3& linVelARO, const b3Vector3& angVelARO, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, const b3Vector3& linVelBRO, const b3Vector3& angVelBRO, float invMassB, const b3Matrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4], b3Vector3& dLinVelA, b3Vector3& dAngVelA, b3Vector3& dLinVelB, b3Vector3& dAngVelB) +static inline void solveFriction(b3GpuConstraint4& cs, + const b3Vector3& posA, const b3Vector3& linVelARO, const b3Vector3& angVelARO, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, const b3Vector3& linVelBRO, const b3Vector3& angVelBRO, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4], b3Vector3& dLinVelA, b3Vector3& dAngVelA, b3Vector3& dLinVelB, b3Vector3& dAngVelB) { + b3Vector3 linVelA = linVelARO + dLinVelA; + b3Vector3 linVelB = linVelBRO + dLinVelB; + b3Vector3 angVelA = angVelARO + dAngVelA; + b3Vector3 angVelB = angVelBRO + dAngVelB; - b3Vector3 linVelA = linVelARO+dLinVelA; - b3Vector3 linVelB = linVelBRO+dLinVelB; - b3Vector3 angVelA = angVelARO+dAngVelA; - b3Vector3 angVelB = angVelBRO+dAngVelB; - - if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; + if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return; const b3Vector3& center = (const b3Vector3&)cs.m_center; b3Vector3 n = -(const b3Vector3&)cs.m_linear; b3Vector3 tangent[2]; -#if 1 - b3PlaneSpace1 (n, tangent[0],tangent[1]); +#if 1 + b3PlaneSpace1(n, tangent[0], tangent[1]); #else - b3Vector3 r = cs.m_worldPos[0]-center; - tangent[0] = cross3( n, r ); - tangent[1] = cross3( tangent[0], n ); - tangent[0] = normalize3( tangent[0] ); - tangent[1] = normalize3( tangent[1] ); + b3Vector3 r = cs.m_worldPos[0] - center; + tangent[0] = cross3(n, r); + tangent[1] = cross3(tangent[0], n); + tangent[0] = normalize3(tangent[0]); + tangent[1] = normalize3(tangent[1]); #endif b3Vector3 angular0, angular1, linear; b3Vector3 r0 = center - posA; b3Vector3 r1 = center - posB; - for(int i=0; i<2; i++) + for (int i = 0; i < 2; i++) { - setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 ); + setLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1); float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB ); + linVelA, angVelA, linVelB, angVelB); rambdaDt *= cs.m_fJacCoeffInv[i]; - { - float prevSum = cs.m_fAppliedRambdaDt[i]; - float updated = prevSum; - updated += rambdaDt; - updated = b3Max( updated, minRambdaDt[i] ); - updated = b3Min( updated, maxRambdaDt[i] ); - rambdaDt = updated - prevSum; - cs.m_fAppliedRambdaDt[i] = updated; - } + { + float prevSum = cs.m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max(updated, minRambdaDt[i]); + updated = b3Min(updated, maxRambdaDt[i]); + rambdaDt = updated - prevSum; + cs.m_fAppliedRambdaDt[i] = updated; + } - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; + b3Vector3 linImp0 = invMassA * linear * rambdaDt; + b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt; + b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt; + b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt; #ifdef _WIN32 b3Assert(_finite(linImp0.getX())); b3Assert(_finite(linImp1.getX())); @@ -326,65 +302,58 @@ static inline void solveFriction(b3GpuConstraint4& cs, } } - { // angular damping for point constraint - b3Vector3 ab = ( posB - posA ).normalized(); - b3Vector3 ac = ( center - posA ).normalized(); - if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { // angular damping for point constraint + b3Vector3 ab = (posB - posA).normalized(); + b3Vector3 ac = (center - posA).normalized(); + if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) { - float angNA = b3Dot( n, angVelA ); - float angNB = b3Dot( n, angVelB ); + float angNA = b3Dot(n, angVelA); + float angNB = b3Dot(n, angVelB); if (invMassA) - dAngVelA -= (angNA*0.1f)*n; + dAngVelA -= (angNA * 0.1f) * n; if (invMassB) - dAngVelB -= (angNB*0.1f)*n; + dAngVelB -= (angNB * 0.1f) * n; } } - } - - - float calcJacCoeff(const b3Vector3& linear0, const b3Vector3& linear1, const b3Vector3& angular0, const b3Vector3& angular1, - float invMass0, const b3Matrix3x3* invInertia0, float invMass1, const b3Matrix3x3* invInertia1, float countA, float countB) + float invMass0, const b3Matrix3x3* invInertia0, float invMass1, const b3Matrix3x3* invInertia1, float countA, float countB) { // linear0,1 are normlized - float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0; - - float jmj1 = b3Dot(mtMul3(angular0,*invInertia0), angular0); - float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1; - float jmj3 = b3Dot(mtMul3(angular1,*invInertia1), angular1); - return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB); -// return -1.f/((jmj0+jmj1)+(jmj2+jmj3)); + float jmj0 = invMass0; //dot3F4(linear0, linear0)*invMass0; + float jmj1 = b3Dot(mtMul3(angular0, *invInertia0), angular0); + float jmj2 = invMass1; //dot3F4(linear1, linear1)*invMass1; + float jmj3 = b3Dot(mtMul3(angular1, *invInertia1), angular1); + return -1.f / ((jmj0 + jmj1) * countA + (jmj2 + jmj3) * countB); + // return -1.f/((jmj0+jmj1)+(jmj2+jmj3)); } - -void setConstraint4( const b3Vector3& posA, const b3Vector3& linVelA, const b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, const b3Vector3& linVelB, const b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, - b3Contact4* src, float dt, float positionDrift, float positionConstraintCoeff, float countA, float countB, - b3GpuConstraint4* dstC ) +void setConstraint4(const b3Vector3& posA, const b3Vector3& linVelA, const b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, const b3Vector3& linVelB, const b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + b3Contact4* src, float dt, float positionDrift, float positionConstraintCoeff, float countA, float countB, + b3GpuConstraint4* 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[3] = 0.7f ;//src->getFrictionCoeff() ); - for(int ic=0; ic<4; ic++) + dstC->m_linear[3] = 0.7f; //src->getFrictionCoeff() ); + for (int ic = 0; ic < 4; ic++) { b3Vector3 r0 = src->m_worldPosB[ic] - posA; b3Vector3 r1 = src->m_worldPosB[ic] - posB; - if( ic >= src->m_worldNormalOnB[3] )//npoints + if (ic >= src->m_worldNormalOnB[3]) //npoints { dstC->m_jacCoeffInv[ic] = 0.f; continue; @@ -396,53 +365,53 @@ void setConstraint4( const b3Vector3& posA, const b3Vector3& linVelA, const b3Ve setLinearAndAngular(src->m_worldNormalOnB, r0, r1, linear, angular0, angular1); dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, - invMassA, &invInertiaA, invMassB, &invInertiaB ,countA,countB); + invMassA, &invInertiaA, invMassB, &invInertiaB, countA, countB); 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 ) + 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_worldPos[ic].w; - dstC->m_b[ic] += (src->m_worldPosB[ic][3] + positionDrift)*positionConstraintCoeff*dtInv; + dstC->m_b[ic] += (src->m_worldPosB[ic][3] + positionDrift) * positionConstraintCoeff * dtInv; dstC->m_appliedRambdaDt[ic] = 0.f; } } - if( src->m_worldNormalOnB[3] > 0 )//npoints - { // prepare friction + if (src->m_worldNormalOnB[3] > 0) //npoints + { // prepare friction b3Vector3 center = make_float4(0.f); - for(int i=0; i<src->m_worldNormalOnB[3]; i++) + for (int i = 0; i < src->m_worldNormalOnB[3]; i++) center += src->m_worldPosB[i]; center /= (float)src->m_worldNormalOnB[3]; b3Vector3 tangent[2]; - b3PlaneSpace1(src->m_worldNormalOnB,tangent[0],tangent[1]); - + b3PlaneSpace1(src->m_worldNormalOnB, tangent[0], tangent[1]); + b3Vector3 r[2]; r[0] = center - posA; r[1] = center - posB; - for(int i=0; i<2; i++) + for (int i = 0; i < 2; i++) { b3Vector3 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 ,countA,countB); + invMassA, &invInertiaA, invMassB, &invInertiaB, countA, countB); 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[3] ) + if (i < src->m_worldNormalOnB[3]) { dstC->m_worldPos[i] = src->m_worldPosB[i]; } @@ -453,17 +422,14 @@ void setConstraint4( const b3Vector3& posA, const b3Vector3& linVelA, const b3Ve } } - - void ContactToConstraintKernel(b3Contact4* gContact, b3RigidBodyData* gBodies, b3InertiaData* gShapes, b3GpuConstraint4* gConstraintOut, int nContacts, -float dt, -float positionDrift, -float positionConstraintCoeff, int gIdx, b3AlignedObjectArray<unsigned int>& bodyCount -) + float dt, + float positionDrift, + float positionConstraintCoeff, int gIdx, b3AlignedObjectArray<unsigned int>& bodyCount) { //int gIdx = 0;//GET_GLOBAL_IDX; - - if( gIdx < nContacts ) + + if (gIdx < nContacts) { int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); @@ -472,50 +438,46 @@ float positionConstraintCoeff, int gIdx, b3AlignedObjectArray<unsigned int>& bod b3Vector3 linVelA = gBodies[aIdx].m_linVel; b3Vector3 angVelA = gBodies[aIdx].m_angVel; float invMassA = gBodies[aIdx].m_invMass; - b3Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertiaWorld;//.m_invInertia; + b3Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertiaWorld; //.m_invInertia; b3Vector3 posB = gBodies[bIdx].m_pos; b3Vector3 linVelB = gBodies[bIdx].m_linVel; b3Vector3 angVelB = gBodies[bIdx].m_angVel; float invMassB = gBodies[bIdx].m_invMass; - b3Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertiaWorld;//m_invInertia; + b3Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertiaWorld; //m_invInertia; b3GpuConstraint4 cs; float countA = invMassA ? (float)(bodyCount[aIdx]) : 1; float countB = invMassB ? (float)(bodyCount[bIdx]) : 1; - setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, - &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB, - &cs ); - + setConstraint4(posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, + &gContact[gIdx], dt, positionDrift, positionConstraintCoeff, countA, countB, + &cs); - cs.m_batchIdx = gContact[gIdx].m_batchIdx; gConstraintOut[gIdx] = cs; } } - -void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo) +void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, const b3JacobiSolverInfo& solverInfo) { B3_PROFILE("b3GpuJacobiContactSolver::solveGroup"); b3AlignedObjectArray<unsigned int> bodyCount; bodyCount.resize(numBodies); - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) bodyCount[i] = 0; b3AlignedObjectArray<b3Int2> contactConstraintOffsets; contactConstraintOffsets.resize(numManifolds); - - for (int i=0;i<numManifolds;i++) + for (int i = 0; i < numManifolds; i++) { int pa = manifoldPtr[i].m_bodyAPtrAndSignBit; int pb = manifoldPtr[i].m_bodyBPtrAndSignBit; - bool isFixedA = (pa <0) || (pa == solverInfo.m_fixedBodyIndex); - bool isFixedB = (pb <0) || (pb == solverInfo.m_fixedBodyIndex); + bool isFixedA = (pa < 0) || (pa == solverInfo.m_fixedBodyIndex); + bool isFixedB = (pb < 0) || (pb == solverInfo.m_fixedBodyIndex); int bodyIndexA = manifoldPtr[i].getBodyA(); int bodyIndexB = manifoldPtr[i].getBodyB(); @@ -529,71 +491,63 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaD { contactConstraintOffsets[i].y = bodyCount[bodyIndexB]; bodyCount[bodyIndexB]++; - } + } } b3AlignedObjectArray<unsigned int> offsetSplitBodies; offsetSplitBodies.resize(numBodies); unsigned int totalNumSplitBodies; - m_data->m_scan->executeHost(bodyCount,offsetSplitBodies,numBodies,&totalNumSplitBodies); - int numlastBody = bodyCount[numBodies-1]; + m_data->m_scan->executeHost(bodyCount, offsetSplitBodies, numBodies, &totalNumSplitBodies); + int numlastBody = bodyCount[numBodies - 1]; totalNumSplitBodies += numlastBody; - printf("totalNumSplitBodies = %d\n",totalNumSplitBodies); - - - - + printf("totalNumSplitBodies = %d\n", totalNumSplitBodies); b3AlignedObjectArray<b3GpuConstraint4> contactConstraints; contactConstraints.resize(numManifolds); - for (int i=0;i<numManifolds;i++) + for (int i = 0; i < numManifolds; i++) { - ContactToConstraintKernel(&manifoldPtr[0],bodies,inertias,&contactConstraints[0],numManifolds, - solverInfo.m_deltaTime, - solverInfo.m_positionDrift, - solverInfo.m_positionConstraintCoeff, - i, bodyCount); + ContactToConstraintKernel(&manifoldPtr[0], bodies, inertias, &contactConstraints[0], numManifolds, + solverInfo.m_deltaTime, + solverInfo.m_positionDrift, + solverInfo.m_positionConstraintCoeff, + i, bodyCount); } int maxIter = solverInfo.m_numIterations; - b3AlignedObjectArray<b3Vector3> deltaLinearVelocities; b3AlignedObjectArray<b3Vector3> deltaAngularVelocities; deltaLinearVelocities.resize(totalNumSplitBodies); deltaAngularVelocities.resize(totalNumSplitBodies); - for (unsigned int i=0;i<totalNumSplitBodies;i++) + for (unsigned int i = 0; i < totalNumSplitBodies; i++) { deltaLinearVelocities[i].setZero(); deltaAngularVelocities[i].setZero(); } - - - for (int iter = 0;iter<maxIter;iter++) + for (int iter = 0; iter < maxIter; iter++) { - int i=0; - for( i=0; i<numManifolds; i++) + int i = 0; + for (i = 0; i < numManifolds; i++) { - //float frictionCoeff = contactConstraints[i].getFrictionCoeff(); int aIdx = (int)contactConstraints[i].m_bodyA; int bIdx = (int)contactConstraints[i].m_bodyB; b3RigidBodyData& bodyA = bodies[aIdx]; b3RigidBodyData& bodyB = bodies[bIdx]; - b3Vector3 zero = b3MakeVector3(0,0,0); - - b3Vector3* dlvAPtr=&zero; - b3Vector3* davAPtr=&zero; - b3Vector3* dlvBPtr=&zero; - b3Vector3* davBPtr=&zero; - + b3Vector3 zero = b3MakeVector3(0, 0, 0); + + b3Vector3* dlvAPtr = &zero; + b3Vector3* davAPtr = &zero; + b3Vector3* dlvBPtr = &zero; + b3Vector3* davBPtr = &zero; + if (bodyA.m_invMass) { int bodyOffsetA = offsetSplitBodies[aIdx]; int constraintOffsetA = contactConstraintOffsets[i].x; - int splitIndexA = bodyOffsetA+constraintOffsetA; + int splitIndexA = bodyOffsetA + constraintOffsetA; dlvAPtr = &deltaLinearVelocities[splitIndexA]; davAPtr = &deltaAngularVelocities[splitIndexA]; } @@ -602,67 +556,61 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaD { int bodyOffsetB = offsetSplitBodies[bIdx]; int constraintOffsetB = contactConstraintOffsets[i].y; - int splitIndexB= bodyOffsetB+constraintOffsetB; - dlvBPtr =&deltaLinearVelocities[splitIndexB]; + int splitIndexB = bodyOffsetB + constraintOffsetB; + dlvBPtr = &deltaLinearVelocities[splitIndexB]; davBPtr = &deltaAngularVelocities[splitIndexB]; } - - { - float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; - float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; - - solveContact( contactConstraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, inertias[aIdx].m_invInertiaWorld, - (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertias[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr ); - + float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX}; + float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f}; + solveContact(contactConstraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, inertias[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertias[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt, *dlvAPtr, *davAPtr, *dlvBPtr, *davBPtr); } - } - //easy - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { if (bodies[i].m_invMass) { int bodyOffset = offsetSplitBodies[i]; int count = bodyCount[i]; - float factor = 1.f/float(count); + float factor = 1.f / float(count); b3Vector3 averageLinVel; averageLinVel.setZero(); b3Vector3 averageAngVel; averageAngVel.setZero(); - for (int j=0;j<count;j++) + for (int j = 0; j < count; j++) { - averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor; - averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor; + averageLinVel += deltaLinearVelocities[bodyOffset + j] * factor; + averageAngVel += deltaAngularVelocities[bodyOffset + j] * factor; } - for (int j=0;j<count;j++) + for (int j = 0; j < count; j++) { - deltaLinearVelocities[bodyOffset+j] = averageLinVel; - deltaAngularVelocities[bodyOffset+j] = averageAngVel; + deltaLinearVelocities[bodyOffset + j] = averageLinVel; + deltaAngularVelocities[bodyOffset + j] = averageAngVel; } } } } - for (int iter = 0;iter<maxIter;iter++) + for (int iter = 0; iter < maxIter; iter++) { //int i=0; - + //solve friction - for(int i=0; i<numManifolds; i++) + for (int i = 0; i < numManifolds; i++) { - float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; - float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; + float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX}; + float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f}; float sum = 0; - for(int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { - sum +=contactConstraints[i].m_appliedRambdaDt[j]; + sum += contactConstraints[i].m_appliedRambdaDt[j]; } float frictionCoeff = contactConstraints[i].getFrictionCoeff(); int aIdx = (int)contactConstraints[i].m_bodyA; @@ -670,18 +618,18 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaD b3RigidBodyData& bodyA = bodies[aIdx]; b3RigidBodyData& bodyB = bodies[bIdx]; - b3Vector3 zero = b3MakeVector3(0,0,0); - - b3Vector3* dlvAPtr=&zero; - b3Vector3* davAPtr=&zero; - b3Vector3* dlvBPtr=&zero; - b3Vector3* davBPtr=&zero; - + b3Vector3 zero = b3MakeVector3(0, 0, 0); + + b3Vector3* dlvAPtr = &zero; + b3Vector3* davAPtr = &zero; + b3Vector3* dlvBPtr = &zero; + b3Vector3* davBPtr = &zero; + if (bodyA.m_invMass) { int bodyOffsetA = offsetSplitBodies[aIdx]; int constraintOffsetA = contactConstraintOffsets[i].x; - int splitIndexA = bodyOffsetA+constraintOffsetA; + int splitIndexA = bodyOffsetA + constraintOffsetA; dlvAPtr = &deltaLinearVelocities[splitIndexA]; davAPtr = &deltaAngularVelocities[splitIndexA]; } @@ -690,55 +638,50 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaD { int bodyOffsetB = offsetSplitBodies[bIdx]; int constraintOffsetB = contactConstraintOffsets[i].y; - int splitIndexB= bodyOffsetB+constraintOffsetB; - dlvBPtr =&deltaLinearVelocities[splitIndexB]; + int splitIndexB = bodyOffsetB + constraintOffsetB; + dlvBPtr = &deltaLinearVelocities[splitIndexB]; davBPtr = &deltaAngularVelocities[splitIndexB]; } - for(int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { - maxRambdaDt[j] = frictionCoeff*sum; + maxRambdaDt[j] = frictionCoeff * sum; minRambdaDt[j] = -maxRambdaDt[j]; } - solveFriction( contactConstraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,inertias[aIdx].m_invInertiaWorld, - (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertias[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr); - + solveFriction(contactConstraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, inertias[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertias[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt, *dlvAPtr, *davAPtr, *dlvBPtr, *davBPtr); } //easy - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { if (bodies[i].m_invMass) { int bodyOffset = offsetSplitBodies[i]; int count = bodyCount[i]; - float factor = 1.f/float(count); + float factor = 1.f / float(count); b3Vector3 averageLinVel; averageLinVel.setZero(); b3Vector3 averageAngVel; averageAngVel.setZero(); - for (int j=0;j<count;j++) + for (int j = 0; j < count; j++) { - averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor; - averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor; + averageLinVel += deltaLinearVelocities[bodyOffset + j] * factor; + averageAngVel += deltaAngularVelocities[bodyOffset + j] * factor; } - for (int j=0;j<count;j++) + for (int j = 0; j < count; j++) { - deltaLinearVelocities[bodyOffset+j] = averageLinVel; - deltaAngularVelocities[bodyOffset+j] = averageAngVel; + deltaLinearVelocities[bodyOffset + j] = averageLinVel; + deltaAngularVelocities[bodyOffset + j] = averageAngVel; } } } - - - } - //easy - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { if (bodies[i].m_invMass) { @@ -753,8 +696,6 @@ void b3GpuJacobiContactSolver::solveGroupHost(b3RigidBodyData* bodies,b3InertiaD } } - - void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index) // // @@ -762,49 +703,47 @@ void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_m { b3JacobiSolverInfo solverInfo; solverInfo.m_fixedBodyIndex = static0Index; - B3_PROFILE("b3GpuJacobiContactSolver::solveGroup"); //int numBodies = bodies->size(); - int numManifolds = numContacts;//manifoldPtr->size(); + int numManifolds = numContacts; //manifoldPtr->size(); { B3_PROFILE("resize"); m_data->m_bodyCount->resize(numBodies); } - - unsigned int val=0; + + unsigned int val = 0; b3Int2 val2; - val2.x=0; - val2.y=0; + val2.x = 0; + val2.y = 0; - { + { B3_PROFILE("m_filler"); m_data->m_contactConstraintOffsets->resize(numManifolds); - m_data->m_filler->execute(*m_data->m_bodyCount,val,numBodies); - - - m_data->m_filler->execute(*m_data->m_contactConstraintOffsets,val2,numManifolds); + m_data->m_filler->execute(*m_data->m_bodyCount, val, numBodies); + + m_data->m_filler->execute(*m_data->m_contactConstraintOffsets, val2, numManifolds); } { B3_PROFILE("m_countBodiesKernel"); - b3LauncherCL launcher(this->m_queue,m_data->m_countBodiesKernel,"m_countBodiesKernel"); - launcher.setBuffer(contactBuf);//manifoldPtr->getBufferCL()); + b3LauncherCL launcher(this->m_queue, m_data->m_countBodiesKernel, "m_countBodiesKernel"); + launcher.setBuffer(contactBuf); //manifoldPtr->getBufferCL()); launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); launcher.setConst(numManifolds); launcher.setConst(solverInfo.m_fixedBodyIndex); launcher.launch1D(numManifolds); } - unsigned int totalNumSplitBodies=0; + unsigned int totalNumSplitBodies = 0; { B3_PROFILE("m_scan->execute"); - + m_data->m_offsetSplitBodies->resize(numBodies); - m_data->m_scan->execute(*m_data->m_bodyCount,*m_data->m_offsetSplitBodies,numBodies,&totalNumSplitBodies); - totalNumSplitBodies+=m_data->m_bodyCount->at(numBodies-1); + m_data->m_scan->execute(*m_data->m_bodyCount, *m_data->m_offsetSplitBodies, numBodies, &totalNumSplitBodies); + totalNumSplitBodies += m_data->m_bodyCount->at(numBodies - 1); } { @@ -812,50 +751,45 @@ void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_m //int numContacts = manifoldPtr->size(); m_data->m_contactConstraints->resize(numContacts); } - + { B3_PROFILE("contactToConstraintSplitKernel"); - b3LauncherCL launcher( m_queue, m_data->m_contactToConstraintSplitKernel,"m_contactToConstraintSplitKernel"); + b3LauncherCL launcher(m_queue, m_data->m_contactToConstraintSplitKernel, "m_contactToConstraintSplitKernel"); launcher.setBuffer(contactBuf); launcher.setBuffer(bodyBuf); launcher.setBuffer(inertiaBuf); launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); - launcher.setConst(numContacts); + launcher.setConst(numContacts); launcher.setConst(solverInfo.m_deltaTime); launcher.setConst(solverInfo.m_positionDrift); launcher.setConst(solverInfo.m_positionConstraintCoeff); - launcher.launch1D( numContacts, 64 ); - + launcher.launch1D(numContacts, 64); } - { B3_PROFILE("m_data->m_deltaLinearVelocities->resize"); m_data->m_deltaLinearVelocities->resize(totalNumSplitBodies); m_data->m_deltaAngularVelocities->resize(totalNumSplitBodies); } - - { B3_PROFILE("m_clearVelocitiesKernel"); - b3LauncherCL launch(m_queue,m_data->m_clearVelocitiesKernel,"m_clearVelocitiesKernel"); + b3LauncherCL launch(m_queue, m_data->m_clearVelocitiesKernel, "m_clearVelocitiesKernel"); launch.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); launch.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); launch.setConst(totalNumSplitBodies); launch.launch1D(totalNumSplitBodies); clFinish(m_queue); } - - + int maxIter = solverInfo.m_numIterations; - for (int iter = 0;iter<maxIter;iter++) + for (int iter = 0; iter < maxIter; iter++) { { B3_PROFILE("m_solveContactKernel"); - b3LauncherCL launcher( m_queue, m_data->m_solveContactKernel,"m_solveContactKernel" ); + b3LauncherCL launcher(m_queue, m_data->m_solveContactKernel, "m_solveContactKernel"); launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); launcher.setBuffer(bodyBuf); launcher.setBuffer(inertiaBuf); @@ -873,11 +807,9 @@ void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_m clFinish(m_queue); } - - { B3_PROFILE("average velocities"); - b3LauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel,"m_averageVelocitiesKernel"); + b3LauncherCL launcher(m_queue, m_data->m_averageVelocitiesKernel, "m_averageVelocitiesKernel"); launcher.setBuffer(bodyBuf); launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); @@ -888,10 +820,9 @@ void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_m clFinish(m_queue); } - { B3_PROFILE("m_solveFrictionKernel"); - b3LauncherCL launcher( m_queue, m_data->m_solveFrictionKernel,"m_solveFrictionKernel"); + b3LauncherCL launcher(m_queue, m_data->m_solveFrictionKernel, "m_solveFrictionKernel"); launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); launcher.setBuffer(bodyBuf); launcher.setBuffer(inertiaBuf); @@ -909,10 +840,9 @@ void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_m clFinish(m_queue); } - { B3_PROFILE("average velocities"); - b3LauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel,"m_averageVelocitiesKernel"); + b3LauncherCL launcher(m_queue, m_data->m_averageVelocitiesKernel, "m_averageVelocitiesKernel"); launcher.setBuffer(bodyBuf); launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); @@ -922,27 +852,20 @@ void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_m launcher.launch1D(numBodies); clFinish(m_queue); } - - - } - { - B3_PROFILE("update body velocities"); - b3LauncherCL launcher( m_queue, m_data->m_updateBodyVelocitiesKernel,"m_updateBodyVelocitiesKernel"); - launcher.setBuffer(bodyBuf); - launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); - launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); - launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); - launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); - launcher.setConst(numBodies); - launcher.launch1D(numBodies); - clFinish(m_queue); - } - - - + B3_PROFILE("update body velocities"); + b3LauncherCL launcher(m_queue, m_data->m_updateBodyVelocitiesKernel, "m_updateBodyVelocitiesKernel"); + launcher.setBuffer(bodyBuf); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_queue); + } } #if 0 diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h index b418f29ec4..8281aee05d 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h @@ -8,7 +8,6 @@ #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" - //struct b3InertiaData; //b3InertiaData @@ -21,21 +20,20 @@ struct b3JacobiSolverInfo float m_deltaTime; float m_positionDrift; float m_positionConstraintCoeff; - int m_numIterations; + int m_numIterations; b3JacobiSolverInfo() - :m_fixedBodyIndex(0), - m_deltaTime(1./60.f), - m_positionDrift( 0.005f ), - m_positionConstraintCoeff( 0.99f ), - m_numIterations(7) + : m_fixedBodyIndex(0), + m_deltaTime(1. / 60.f), + m_positionDrift(0.005f), + m_positionConstraintCoeff(0.99f), + m_numIterations(7) { } }; class b3GpuJacobiContactSolver { protected: - struct b3GpuJacobiSolverInternalData* m_data; cl_context m_context; @@ -43,20 +41,16 @@ protected: cl_command_queue m_queue; public: - b3GpuJacobiContactSolver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity); virtual ~b3GpuJacobiContactSolver(); - void solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index); - void solveGroupHost(b3RigidBodyData* bodies,b3InertiaData* inertias,int numBodies,struct b3Contact4* manifoldPtr, int numManifolds,const b3JacobiSolverInfo& solverInfo); + void solveGroupHost(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, struct b3Contact4* manifoldPtr, int numManifolds, const b3JacobiSolverInfo& solverInfo); //void solveGroupHost(btRigidBodyCL* bodies,b3InertiaData* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo); //b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies,b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); //void solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo); //void solveGroupMixed(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo); - }; -#endif //B3_GPU_JACOBI_CONTACT_SOLVER_H - +#endif //B3_GPU_JACOBI_CONTACT_SOLVER_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp index 698fa15f96..2e4f6c1572 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp @@ -1,6 +1,5 @@ #include "b3GpuNarrowPhase.h" - #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" #include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h" @@ -16,107 +15,87 @@ #include "Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h" #include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h" - - - b3GpuNarrowPhase::b3GpuNarrowPhase(cl_context ctx, cl_device_id device, cl_command_queue queue, const b3Config& config) -:m_data(0) ,m_planeBodyIndex(-1),m_static0Index(-1), -m_context(ctx), -m_device(device), -m_queue(queue) + : m_data(0), m_planeBodyIndex(-1), m_static0Index(-1), m_context(ctx), m_device(device), m_queue(queue) { - m_data = new b3GpuNarrowPhaseInternalData(); m_data->m_currentContactBuffer = 0; - memset(m_data,0,sizeof(b3GpuNarrowPhaseInternalData)); - + memset(m_data, 0, sizeof(b3GpuNarrowPhaseInternalData)); m_data->m_config = config; - - m_data->m_gpuSatCollision = new GpuSatCollision(ctx,device,queue); - - - m_data->m_triangleConvexPairs = new b3OpenCLArray<b3Int4>(m_context,m_queue, config.m_maxTriConvexPairCapacity); + m_data->m_gpuSatCollision = new GpuSatCollision(ctx, device, queue); + + m_data->m_triangleConvexPairs = new b3OpenCLArray<b3Int4>(m_context, m_queue, config.m_maxTriConvexPairCapacity); //m_data->m_convexPairsOutGPU = new b3OpenCLArray<b3Int2>(ctx,queue,config.m_maxBroadphasePairs,false); //m_data->m_planePairs = new b3OpenCLArray<b3Int2>(ctx,queue,config.m_maxBroadphasePairs,false); - + m_data->m_pBufContactOutCPU = new b3AlignedObjectArray<b3Contact4>(); m_data->m_pBufContactOutCPU->resize(config.m_maxBroadphasePairs); m_data->m_bodyBufferCPU = new b3AlignedObjectArray<b3RigidBodyData>(); m_data->m_bodyBufferCPU->resize(config.m_maxConvexBodies); - + m_data->m_inertiaBufferCPU = new b3AlignedObjectArray<b3InertiaData>(); m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies); - - m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true); - m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true); - - m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx,queue,config.m_maxConvexBodies,false); - m_data->m_collidablesGPU = new b3OpenCLArray<b3Collidable>(ctx,queue,config.m_maxConvexShapes); + + m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray<b3Contact4>(ctx, queue, config.m_maxContactCapacity, true); + m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray<b3Contact4>(ctx, queue, config.m_maxContactCapacity, true); + + m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx, queue, config.m_maxConvexBodies, false); + m_data->m_collidablesGPU = new b3OpenCLArray<b3Collidable>(ctx, queue, config.m_maxConvexShapes); m_data->m_collidablesCPU.reserve(config.m_maxConvexShapes); m_data->m_localShapeAABBCPU = new b3AlignedObjectArray<b3SapAabb>; - m_data->m_localShapeAABBGPU = new b3OpenCLArray<b3SapAabb>(ctx,queue,config.m_maxConvexShapes); - - + m_data->m_localShapeAABBGPU = new b3OpenCLArray<b3SapAabb>(ctx, queue, config.m_maxConvexShapes); + //m_data->m_solverDataGPU = adl::Solver<adl::TYPE_CL>::allocate(ctx,queue, config.m_maxBroadphasePairs,false); - m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx,queue, config.m_maxConvexBodies,false); + m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx, queue, config.m_maxConvexBodies, false); - m_data->m_convexFacesGPU = new b3OpenCLArray<b3GpuFace>(ctx,queue,config.m_maxConvexShapes*config.m_maxFacesPerShape,false); - m_data->m_convexFaces.reserve(config.m_maxConvexShapes*config.m_maxFacesPerShape); + m_data->m_convexFacesGPU = new b3OpenCLArray<b3GpuFace>(ctx, queue, config.m_maxConvexShapes * config.m_maxFacesPerShape, false); + m_data->m_convexFaces.reserve(config.m_maxConvexShapes * config.m_maxFacesPerShape); - m_data->m_gpuChildShapes = new b3OpenCLArray<b3GpuChildShape>(ctx,queue,config.m_maxCompoundChildShapes,false); - - m_data->m_convexPolyhedraGPU = new b3OpenCLArray<b3ConvexPolyhedronData>(ctx,queue,config.m_maxConvexShapes,false); + m_data->m_gpuChildShapes = new b3OpenCLArray<b3GpuChildShape>(ctx, queue, config.m_maxCompoundChildShapes, false); + + m_data->m_convexPolyhedraGPU = new b3OpenCLArray<b3ConvexPolyhedronData>(ctx, queue, config.m_maxConvexShapes, false); m_data->m_convexPolyhedra.reserve(config.m_maxConvexShapes); - m_data->m_uniqueEdgesGPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexUniqueEdges,true); + m_data->m_uniqueEdgesGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexUniqueEdges, true); m_data->m_uniqueEdges.reserve(config.m_maxConvexUniqueEdges); - - - m_data->m_convexVerticesGPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexVertices,true); + m_data->m_convexVerticesGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexVertices, true); m_data->m_convexVertices.reserve(config.m_maxConvexVertices); - m_data->m_convexIndicesGPU = new b3OpenCLArray<int>(ctx,queue,config.m_maxConvexIndices,true); - m_data->m_convexIndices.reserve(config.m_maxConvexIndices); - - m_data->m_worldVertsB1GPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); - m_data->m_clippingFacesOutGPU = new b3OpenCLArray<b3Int4>(ctx,queue,config.m_maxConvexBodies); - m_data->m_worldNormalsAGPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies); - m_data->m_worldVertsA1GPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); - m_data->m_worldVertsB2GPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace); - - + m_data->m_convexIndicesGPU = new b3OpenCLArray<int>(ctx, queue, config.m_maxConvexIndices, true); + m_data->m_convexIndices.reserve(config.m_maxConvexIndices); + + m_data->m_worldVertsB1GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace); + m_data->m_clippingFacesOutGPU = new b3OpenCLArray<b3Int4>(ctx, queue, config.m_maxConvexBodies); + m_data->m_worldNormalsAGPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies); + m_data->m_worldVertsA1GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace); + m_data->m_worldVertsB2GPU = new b3OpenCLArray<b3Vector3>(ctx, queue, config.m_maxConvexBodies * config.m_maxVerticesPerFace); - m_data->m_convexData = new b3AlignedObjectArray<b3ConvexUtility* >(); + m_data->m_convexData = new b3AlignedObjectArray<b3ConvexUtility*>(); m_data->m_convexData->resize(config.m_maxConvexShapes); m_data->m_convexPolyhedra.resize(config.m_maxConvexShapes); - + m_data->m_numAcceleratedShapes = 0; m_data->m_numAcceleratedRigidBodies = 0; - - - m_data->m_subTreesGPU = new b3OpenCLArray<b3BvhSubtreeInfo>(this->m_context,this->m_queue); - m_data->m_treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context,this->m_queue); - m_data->m_bvhInfoGPU = new b3OpenCLArray<b3BvhInfo>(this->m_context,this->m_queue); + + m_data->m_subTreesGPU = new b3OpenCLArray<b3BvhSubtreeInfo>(this->m_context, this->m_queue); + m_data->m_treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context, this->m_queue); + m_data->m_bvhInfoGPU = new b3OpenCLArray<b3BvhInfo>(this->m_context, this->m_queue); //m_data->m_contactCGPU = new b3OpenCLArray<Constraint4>(ctx,queue,config.m_maxBroadphasePairs,false); //m_data->m_frictionCGPU = new b3OpenCLArray<adl::Solver<adl::TYPE_CL>::allocateFrictionConstraint( m_data->m_deviceCL, config.m_maxBroadphasePairs); - - - } - b3GpuNarrowPhase::~b3GpuNarrowPhase() { delete m_data->m_gpuSatCollision; - + delete m_data->m_triangleConvexPairs; //delete m_data->m_convexPairsOutGPU; //delete m_data->m_planePairs; @@ -126,7 +105,6 @@ b3GpuNarrowPhase::~b3GpuNarrowPhase() delete m_data->m_pBufContactBuffersGPU[0]; delete m_data->m_pBufContactBuffersGPU[1]; - delete m_data->m_inertiaBufferGPU; delete m_data->m_collidablesGPU; delete m_data->m_localShapeAABBCPU; @@ -139,18 +117,18 @@ b3GpuNarrowPhase::~b3GpuNarrowPhase() delete m_data->m_convexVerticesGPU; delete m_data->m_convexIndicesGPU; delete m_data->m_worldVertsB1GPU; - delete m_data->m_clippingFacesOutGPU; - delete m_data->m_worldNormalsAGPU; + delete m_data->m_clippingFacesOutGPU; + delete m_data->m_worldNormalsAGPU; delete m_data->m_worldVertsA1GPU; - delete m_data->m_worldVertsB2GPU; - + delete m_data->m_worldVertsB2GPU; + delete m_data->m_bvhInfoGPU; - for (int i=0;i<m_data->m_bvhData.size();i++) + for (int i = 0; i < m_data->m_bvhData.size(); i++) { delete m_data->m_bvhData[i]; } - for (int i=0;i<m_data->m_meshInterfaces.size();i++) + for (int i = 0; i < m_data->m_meshInterfaces.size(); i++) { delete m_data->m_meshInterfaces[i]; } @@ -159,198 +137,180 @@ b3GpuNarrowPhase::~b3GpuNarrowPhase() delete m_data->m_treeNodesGPU; delete m_data->m_subTreesGPU; - - delete m_data->m_convexData; + delete m_data->m_convexData; delete m_data; } - -int b3GpuNarrowPhase::allocateCollidable() +int b3GpuNarrowPhase::allocateCollidable() { int curSize = m_data->m_collidablesCPU.size(); - if (curSize<m_data->m_config.m_maxConvexShapes) + if (curSize < m_data->m_config.m_maxConvexShapes) { m_data->m_collidablesCPU.expand(); return curSize; } else { - b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes); + b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes); } return -1; - } - - - - -int b3GpuNarrowPhase::registerSphereShape(float radius) +int b3GpuNarrowPhase::registerSphereShape(float radius) { int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; - b3Collidable& col = getCollidableCpu(collidableIndex); col.m_shapeType = SHAPE_SPHERE; col.m_shapeIndex = 0; col.m_radius = radius; - - if (col.m_shapeIndex>=0) + + if (col.m_shapeIndex >= 0) { b3SapAabb aabb; - b3Vector3 myAabbMin=b3MakeVector3(-radius,-radius,-radius); - b3Vector3 myAabbMax=b3MakeVector3(radius,radius,radius); + b3Vector3 myAabbMin = b3MakeVector3(-radius, -radius, -radius); + b3Vector3 myAabbMax = b3MakeVector3(radius, radius, radius); - aabb.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x; - aabb.m_min[1] = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y; - aabb.m_min[2] = myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z; + aabb.m_min[0] = myAabbMin[0]; //s_convexHeightField->m_aabb.m_min.x; + aabb.m_min[1] = myAabbMin[1]; //s_convexHeightField->m_aabb.m_min.y; + aabb.m_min[2] = myAabbMin[2]; //s_convexHeightField->m_aabb.m_min.z; aabb.m_minIndices[3] = 0; - aabb.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x; - aabb.m_max[1] = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y; - aabb.m_max[2] = myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z; + aabb.m_max[0] = myAabbMax[0]; //s_convexHeightField->m_aabb.m_max.x; + aabb.m_max[1] = myAabbMax[1]; //s_convexHeightField->m_aabb.m_max.y; + aabb.m_max[2] = myAabbMax[2]; //s_convexHeightField->m_aabb.m_max.z; aabb.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabb); -// m_data->m_localShapeAABBGPU->push_back(aabb); + // m_data->m_localShapeAABBGPU->push_back(aabb); clFinish(m_queue); } - + return collidableIndex; } - int b3GpuNarrowPhase::registerFace(const b3Vector3& faceNormal, float faceConstant) { int faceOffset = m_data->m_convexFaces.size(); b3GpuFace& face = m_data->m_convexFaces.expand(); - face.m_plane = b3MakeVector3(faceNormal.x,faceNormal.y,faceNormal.z,faceConstant); + face.m_plane = b3MakeVector3(faceNormal.x, faceNormal.y, faceNormal.z, faceConstant); return faceOffset; } -int b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float planeConstant) +int b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float planeConstant) { int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; - b3Collidable& col = getCollidableCpu(collidableIndex); col.m_shapeType = SHAPE_PLANE; - col.m_shapeIndex = registerFace(planeNormal,planeConstant); + col.m_shapeIndex = registerFace(planeNormal, planeConstant); col.m_radius = planeConstant; - - if (col.m_shapeIndex>=0) + + if (col.m_shapeIndex >= 0) { b3SapAabb aabb; aabb.m_min[0] = -1e30f; aabb.m_min[1] = -1e30f; aabb.m_min[2] = -1e30f; aabb.m_minIndices[3] = 0; - + aabb.m_max[0] = 1e30f; aabb.m_max[1] = 1e30f; aabb.m_max[2] = 1e30f; aabb.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabb); -// m_data->m_localShapeAABBGPU->push_back(aabb); + // m_data->m_localShapeAABBGPU->push_back(aabb); clFinish(m_queue); } - + return collidableIndex; } - -int b3GpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col) +int b3GpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col) { + m_data->m_convexData->resize(m_data->m_numAcceleratedShapes + 1); + m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1); - m_data->m_convexData->resize(m_data->m_numAcceleratedShapes+1); - m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); - - - b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1); convex.mC = convexPtr->mC; convex.mE = convexPtr->mE; - convex.m_extents= convexPtr->m_extents; + convex.m_extents = convexPtr->m_extents; convex.m_localCenter = convexPtr->m_localCenter; convex.m_radius = convexPtr->m_radius; - + convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size(); int edgeOffset = m_data->m_uniqueEdges.size(); convex.m_uniqueEdgesOffset = edgeOffset; - - m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges); - + + m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges); + //convex data here int i; - for ( i=0;i<convexPtr->m_uniqueEdges.size();i++) + for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++) { - m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i]; + m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i]; } - + int faceOffset = m_data->m_convexFaces.size(); convex.m_faceOffset = faceOffset; convex.m_numFaces = convexPtr->m_faces.size(); - m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces); - + m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces); - for (i=0;i<convexPtr->m_faces.size();i++) + for (i = 0; i < convexPtr->m_faces.size(); i++) { - m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0], - convexPtr->m_faces[i].m_plane[1], - convexPtr->m_faces[i].m_plane[2], - convexPtr->m_faces[i].m_plane[3]); + m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0], + convexPtr->m_faces[i].m_plane[1], + convexPtr->m_faces[i].m_plane[2], + convexPtr->m_faces[i].m_plane[3]); - int indexOffset = m_data->m_convexIndices.size(); int numIndices = convexPtr->m_faces[i].m_indices.size(); - m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices; - m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset; - m_data->m_convexIndices.resize(indexOffset+numIndices); - for (int p=0;p<numIndices;p++) + m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices; + m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset; + m_data->m_convexIndices.resize(indexOffset + numIndices); + for (int p = 0; p < numIndices; p++) { - m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p]; + m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p]; } } - + convex.m_numVertices = convexPtr->m_vertices.size(); int vertexOffset = m_data->m_convexVertices.size(); - convex.m_vertexOffset =vertexOffset; - - m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices); - for (int i=0;i<convexPtr->m_vertices.size();i++) + convex.m_vertexOffset = vertexOffset; + + m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices); + for (int i = 0; i < convexPtr->m_vertices.size(); i++) { - m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i]; + m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i]; } (*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr; - - - + return m_data->m_numAcceleratedShapes++; } - -int b3GpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling) +int b3GpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling) { b3AlignedObjectArray<b3Vector3> verts; - unsigned char* vts = (unsigned char*) vertices; - for (int i=0;i<numVertices;i++) + unsigned char* vts = (unsigned char*)vertices; + for (int i = 0; i < numVertices; i++) { - float* vertex = (float*) &vts[i*strideInBytes]; - verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2])); + float* vertex = (float*)&vts[i * strideInBytes]; + verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2])); } b3ConvexUtility* utilPtr = new b3ConvexUtility(); bool merge = true; if (numVertices) { - utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge); + utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge); } int collidableIndex = registerConvexHullShape(utilPtr); @@ -358,35 +318,34 @@ int b3GpuNarrowPhase::registerConvexHullShape(const float* vertices, int stride return collidableIndex; } -int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) +int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) { int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; b3Collidable& col = getCollidableCpu(collidableIndex); col.m_shapeType = SHAPE_CONVEX_HULL; col.m_shapeIndex = -1; - - + { - b3Vector3 localCenter=b3MakeVector3(0,0,0); - for (int i=0;i<utilPtr->m_vertices.size();i++) - localCenter+=utilPtr->m_vertices[i]; - localCenter*= (1.f/utilPtr->m_vertices.size()); + b3Vector3 localCenter = b3MakeVector3(0, 0, 0); + for (int i = 0; i < utilPtr->m_vertices.size(); i++) + localCenter += utilPtr->m_vertices[i]; + localCenter *= (1.f / utilPtr->m_vertices.size()); utilPtr->m_localCenter = localCenter; - col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col); + col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col); } - if (col.m_shapeIndex>=0) + if (col.m_shapeIndex >= 0) { b3SapAabb aabb; - - b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); - b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); - for (int i=0;i<utilPtr->m_vertices.size();i++) + b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f); + b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f); + + for (int i = 0; i < utilPtr->m_vertices.size(); i++) { myAabbMin.setMin(utilPtr->m_vertices[i]); myAabbMax.setMax(utilPtr->m_vertices[i]); @@ -402,18 +361,16 @@ int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) aabb.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabb); -// m_data->m_localShapeAABBGPU->push_back(aabb); + // m_data->m_localShapeAABBGPU->push_back(aabb); } - - return collidableIndex; + return collidableIndex; } -int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes) +int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes) { - int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; b3Collidable& col = getCollidableCpu(collidableIndex); @@ -422,44 +379,41 @@ int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShap col.m_compoundBvhIndex = m_data->m_bvhInfoCPU.size(); { - b3Assert(col.m_shapeIndex+childShapes->size()<m_data->m_config.m_maxCompoundChildShapes); - for (int i=0;i<childShapes->size();i++) + b3Assert(col.m_shapeIndex + childShapes->size() < m_data->m_config.m_maxCompoundChildShapes); + for (int i = 0; i < childShapes->size(); i++) { m_data->m_cpuChildShapes.push_back(childShapes->at(i)); } } - - col.m_numChildShapes = childShapes->size(); - - + b3SapAabb aabbLocalSpace; - b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); - b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); - + b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f); + b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f); + b3AlignedObjectArray<b3Aabb> childLocalAabbs; childLocalAabbs.resize(childShapes->size()); //compute local AABB of the compound of all children - for (int i=0;i<childShapes->size();i++) + for (int i = 0; i < childShapes->size(); i++) { int childColIndex = childShapes->at(i).m_shapeIndex; //b3Collidable& childCol = getCollidableCpu(childColIndex); - b3SapAabb aabbLoc =m_data->m_localShapeAABBCPU->at(childColIndex); + b3SapAabb aabbLoc = m_data->m_localShapeAABBCPU->at(childColIndex); - b3Vector3 childLocalAabbMin=b3MakeVector3(aabbLoc.m_min[0],aabbLoc.m_min[1],aabbLoc.m_min[2]); - b3Vector3 childLocalAabbMax=b3MakeVector3(aabbLoc.m_max[0],aabbLoc.m_max[1],aabbLoc.m_max[2]); - b3Vector3 aMin,aMax; + b3Vector3 childLocalAabbMin = b3MakeVector3(aabbLoc.m_min[0], aabbLoc.m_min[1], aabbLoc.m_min[2]); + b3Vector3 childLocalAabbMax = b3MakeVector3(aabbLoc.m_max[0], aabbLoc.m_max[1], aabbLoc.m_max[2]); + b3Vector3 aMin, aMax; b3Scalar margin(0.f); b3Transform childTr; childTr.setIdentity(); childTr.setOrigin(childShapes->at(i).m_childPosition); childTr.setRotation(b3Quaternion(childShapes->at(i).m_childOrientation)); - b3TransformAabb(childLocalAabbMin,childLocalAabbMax,margin,childTr,aMin,aMax); + b3TransformAabb(childLocalAabbMin, childLocalAabbMax, margin, childTr, aMin, aMax); myAabbMin.setMin(aMin); - myAabbMax.setMax(aMax); + myAabbMax.setMax(aMax); childLocalAabbs[i].m_min[0] = aMin[0]; childLocalAabbs[i].m_min[1] = aMin[1]; childLocalAabbs[i].m_min[2] = aMin[2]; @@ -469,36 +423,35 @@ int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShap childLocalAabbs[i].m_max[2] = aMax[2]; childLocalAabbs[i].m_max[3] = 0; } - - aabbLocalSpace.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x; - aabbLocalSpace.m_min[1]= myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y; - aabbLocalSpace.m_min[2]= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z; + + aabbLocalSpace.m_min[0] = myAabbMin[0]; //s_convexHeightField->m_aabb.m_min.x; + aabbLocalSpace.m_min[1] = myAabbMin[1]; //s_convexHeightField->m_aabb.m_min.y; + aabbLocalSpace.m_min[2] = myAabbMin[2]; //s_convexHeightField->m_aabb.m_min.z; aabbLocalSpace.m_minIndices[3] = 0; - - aabbLocalSpace.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x; - aabbLocalSpace.m_max[1]= myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y; - aabbLocalSpace.m_max[2]= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z; + + aabbLocalSpace.m_max[0] = myAabbMax[0]; //s_convexHeightField->m_aabb.m_max.x; + aabbLocalSpace.m_max[1] = myAabbMax[1]; //s_convexHeightField->m_aabb.m_max.y; + aabbLocalSpace.m_max[2] = myAabbMax[2]; //s_convexHeightField->m_aabb.m_max.z; aabbLocalSpace.m_signedMaxIndices[3] = 0; - - m_data->m_localShapeAABBCPU->push_back(aabbLocalSpace); + m_data->m_localShapeAABBCPU->push_back(aabbLocalSpace); b3QuantizedBvh* bvh = new b3QuantizedBvh; - bvh->setQuantizationValues(myAabbMin,myAabbMax); - QuantizedNodeArray& nodes = bvh->getLeafNodeArray(); + bvh->setQuantizationValues(myAabbMin, myAabbMax); + QuantizedNodeArray& nodes = bvh->getLeafNodeArray(); int numNodes = childShapes->size(); - for (int i=0;i<numNodes;i++) + for (int i = 0; i < numNodes; i++) { b3QuantizedBvhNode node; - b3Vector3 aabbMin,aabbMax; - aabbMin = (b3Vector3&) childLocalAabbs[i].m_min; - aabbMax = (b3Vector3&) childLocalAabbs[i].m_max; + b3Vector3 aabbMin, aabbMax; + aabbMin = (b3Vector3&)childLocalAabbs[i].m_min; + aabbMax = (b3Vector3&)childLocalAabbs[i].m_max; - bvh->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); - bvh->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + bvh->quantize(&node.m_quantizedAabbMin[0], aabbMin, 0); + bvh->quantize(&node.m_quantizedAabbMax[0], aabbMax, 1); int partId = 0; - node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; + node.m_escapeIndexOrTriangleIndex = (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | i; nodes.push_back(node); } bvh->buildInternal(); @@ -511,7 +464,7 @@ int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShap //void buildInternal(); b3BvhInfo bvhInfo; - + bvhInfo.m_aabbMin = bvh->m_bvhAabbMin; bvhInfo.m_aabbMax = bvh->m_bvhAabbMax; bvhInfo.m_quantization = bvh->m_bvhQuantization; @@ -520,80 +473,72 @@ int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShap bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size(); bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size(); - int numNewNodes = bvh->getQuantizedNodeArray().size(); + int numNewNodes = bvh->getQuantizedNodeArray().size(); - for (int i=0;i<numNewNodes-1;i++) + for (int i = 0; i < numNewNodes - 1; i++) { - if (bvh->getQuantizedNodeArray()[i].isLeafNode()) { int orgIndex = bvh->getQuantizedNodeArray()[i].getTriangleIndex(); b3Vector3 nodeMinVec = bvh->unQuantize(bvh->getQuantizedNodeArray()[i].m_quantizedAabbMin); b3Vector3 nodeMaxVec = bvh->unQuantize(bvh->getQuantizedNodeArray()[i].m_quantizedAabbMax); - - for (int c=0;c<3;c++) + + for (int c = 0; c < 3; c++) { if (childLocalAabbs[orgIndex].m_min[c] < nodeMinVec[c]) { - printf("min org (%f) and new (%f) ? at i:%d,c:%d\n",childLocalAabbs[i].m_min[c],nodeMinVec[c],i,c); + printf("min org (%f) and new (%f) ? at i:%d,c:%d\n", childLocalAabbs[i].m_min[c], nodeMinVec[c], i, c); } if (childLocalAabbs[orgIndex].m_max[c] > nodeMaxVec[c]) { - printf("max org (%f) and new (%f) ? at i:%d,c:%d\n",childLocalAabbs[i].m_max[c],nodeMaxVec[c],i,c); + printf("max org (%f) and new (%f) ? at i:%d,c:%d\n", childLocalAabbs[i].m_max[c], nodeMaxVec[c], i, c); } - } } - } m_data->m_bvhInfoCPU.push_back(bvhInfo); int numNewSubtrees = bvh->getSubtreeInfoArray().size(); - m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size()+numNewSubtrees); - for (int i=0;i<numNewSubtrees;i++) + m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size() + numNewSubtrees); + for (int i = 0; i < numNewSubtrees; i++) { m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]); } int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); - for (int i=0;i<numNewTreeNodes;i++) + for (int i = 0; i < numNewTreeNodes; i++) { m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]); } -// m_data->m_localShapeAABBGPU->push_back(aabbWS); + // m_data->m_localShapeAABBGPU->push_back(aabbWS); clFinish(m_queue); return collidableIndex; - } - -int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling1) +int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling1) { - - - b3Vector3 scaling=b3MakeVector3(scaling1[0],scaling1[1],scaling1[2]); + b3Vector3 scaling = b3MakeVector3(scaling1[0], scaling1[1], scaling1[2]); int collidableIndex = allocateCollidable(); - if (collidableIndex<0) + if (collidableIndex < 0) return collidableIndex; b3Collidable& col = getCollidableCpu(collidableIndex); - + col.m_shapeType = SHAPE_CONCAVE_TRIMESH; - col.m_shapeIndex = registerConcaveMeshShape(vertices,indices,col,scaling); + col.m_shapeIndex = registerConcaveMeshShape(vertices, indices, col, scaling); col.m_bvhIndex = m_data->m_bvhInfoCPU.size(); - b3SapAabb aabb; - b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); - b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f); + b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f); - for (int i=0;i<vertices->size();i++) + for (int i = 0; i < vertices->size(); i++) { - b3Vector3 vtx(vertices->at(i)*scaling); + b3Vector3 vtx(vertices->at(i) * scaling); myAabbMin.setMin(vtx); myAabbMax.setMax(vtx); } @@ -603,27 +548,27 @@ int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vert aabb.m_minIndices[3] = 0; aabb.m_max[0] = myAabbMax[0]; - aabb.m_max[1]= myAabbMax[1]; - aabb.m_max[2]= myAabbMax[2]; - aabb.m_signedMaxIndices[3]= 0; + aabb.m_max[1] = myAabbMax[1]; + aabb.m_max[2] = myAabbMax[2]; + aabb.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabb); -// m_data->m_localShapeAABBGPU->push_back(aabb); + // m_data->m_localShapeAABBGPU->push_back(aabb); b3OptimizedBvh* bvh = new b3OptimizedBvh(); //void b3OptimizedBvh::build(b3StridingMeshInterface* triangles, bool useQuantizedAabbCompression, const b3Vector3& bvhAabbMin, const b3Vector3& bvhAabbMax) - + bool useQuantizedAabbCompression = true; - b3TriangleIndexVertexArray* meshInterface=new b3TriangleIndexVertexArray(); + b3TriangleIndexVertexArray* meshInterface = new b3TriangleIndexVertexArray(); m_data->m_meshInterfaces.push_back(meshInterface); b3IndexedMesh mesh; - mesh.m_numTriangles = indices->size()/3; + mesh.m_numTriangles = indices->size() / 3; mesh.m_numVertices = vertices->size(); - mesh.m_vertexBase = (const unsigned char *)&vertices->at(0).x; + mesh.m_vertexBase = (const unsigned char*)&vertices->at(0).x; mesh.m_vertexStride = sizeof(b3Vector3); - mesh.m_triangleIndexStride = 3 * sizeof(int);// or sizeof(int) - mesh.m_triangleIndexBase = (const unsigned char *)&indices->at(0); - + mesh.m_triangleIndexStride = 3 * sizeof(int); // or sizeof(int) + mesh.m_triangleIndexBase = (const unsigned char*)&indices->at(0); + meshInterface->addIndexedMesh(mesh); bvh->build(meshInterface, useQuantizedAabbCompression, (b3Vector3&)aabb.m_min, (b3Vector3&)aabb.m_max); m_data->m_bvhData.push_back(bvh); @@ -632,7 +577,7 @@ int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vert int numSubTrees = bvh->getSubtreeInfoArray().size(); b3BvhInfo bvhInfo; - + bvhInfo.m_aabbMin = bvh->m_bvhAabbMin; bvhInfo.m_aabbMax = bvh->m_bvhAabbMax; bvhInfo.m_quantization = bvh->m_bvhQuantization; @@ -643,97 +588,87 @@ int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vert m_data->m_bvhInfoCPU.push_back(bvhInfo); - int numNewSubtrees = bvh->getSubtreeInfoArray().size(); - m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size()+numNewSubtrees); - for (int i=0;i<numNewSubtrees;i++) + m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size() + numNewSubtrees); + for (int i = 0; i < numNewSubtrees; i++) { m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]); } int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); - for (int i=0;i<numNewTreeNodes;i++) + for (int i = 0; i < numNewTreeNodes; i++) { m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]); } - - - return collidableIndex; } -int b3GpuNarrowPhase::registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,b3Collidable& col, const float* scaling1) +int b3GpuNarrowPhase::registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling1) { + b3Vector3 scaling = b3MakeVector3(scaling1[0], scaling1[1], scaling1[2]); + m_data->m_convexData->resize(m_data->m_numAcceleratedShapes + 1); + m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1); - b3Vector3 scaling=b3MakeVector3(scaling1[0],scaling1[1],scaling1[2]); - - m_data->m_convexData->resize(m_data->m_numAcceleratedShapes+1); - m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1); - - - b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1); - convex.mC = b3MakeVector3(0,0,0); - convex.mE = b3MakeVector3(0,0,0); - convex.m_extents= b3MakeVector3(0,0,0); - convex.m_localCenter = b3MakeVector3(0,0,0); + b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1); + convex.mC = b3MakeVector3(0, 0, 0); + convex.mE = b3MakeVector3(0, 0, 0); + convex.m_extents = b3MakeVector3(0, 0, 0); + convex.m_localCenter = b3MakeVector3(0, 0, 0); convex.m_radius = 0.f; - + convex.m_numUniqueEdges = 0; int edgeOffset = m_data->m_uniqueEdges.size(); convex.m_uniqueEdgesOffset = edgeOffset; - + int faceOffset = m_data->m_convexFaces.size(); convex.m_faceOffset = faceOffset; - - convex.m_numFaces = indices->size()/3; - m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces); - m_data->m_convexIndices.reserve(convex.m_numFaces*3); - for (int i=0;i<convex.m_numFaces;i++) + + convex.m_numFaces = indices->size() / 3; + m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces); + m_data->m_convexIndices.reserve(convex.m_numFaces * 3); + for (int i = 0; i < convex.m_numFaces; i++) { - if (i%256==0) + if (i % 256 == 0) { //printf("i=%d out of %d", i,convex.m_numFaces); } - b3Vector3 vert0(vertices->at(indices->at(i*3))*scaling); - b3Vector3 vert1(vertices->at(indices->at(i*3+1))*scaling); - b3Vector3 vert2(vertices->at(indices->at(i*3+2))*scaling); + b3Vector3 vert0(vertices->at(indices->at(i * 3)) * scaling); + b3Vector3 vert1(vertices->at(indices->at(i * 3 + 1)) * scaling); + b3Vector3 vert2(vertices->at(indices->at(i * 3 + 2)) * scaling); - b3Vector3 normal = ((vert1-vert0).cross(vert2-vert0)).normalize(); + b3Vector3 normal = ((vert1 - vert0).cross(vert2 - vert0)).normalize(); b3Scalar c = -(normal.dot(vert0)); - m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector4(normal.x,normal.y,normal.z,c); + m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector4(normal.x, normal.y, normal.z, c); int indexOffset = m_data->m_convexIndices.size(); int numIndices = 3; - m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices; - m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset; - m_data->m_convexIndices.resize(indexOffset+numIndices); - for (int p=0;p<numIndices;p++) + m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices; + m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset; + m_data->m_convexIndices.resize(indexOffset + numIndices); + for (int p = 0; p < numIndices; p++) { - int vi = indices->at(i*3+p); - m_data->m_convexIndices[indexOffset+p] = vi;//convexPtr->m_faces[i].m_indices[p]; + int vi = indices->at(i * 3 + p); + m_data->m_convexIndices[indexOffset + p] = vi; //convexPtr->m_faces[i].m_indices[p]; } } - + convex.m_numVertices = vertices->size(); int vertexOffset = m_data->m_convexVertices.size(); - convex.m_vertexOffset =vertexOffset; - m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices); - for (int i=0;i<vertices->size();i++) + convex.m_vertexOffset = vertexOffset; + m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices); + for (int i = 0; i < vertices->size(); i++) { - m_data->m_convexVertices[vertexOffset+i] = vertices->at(i)*scaling; + m_data->m_convexVertices[vertexOffset + i] = vertices->at(i) * scaling; } (*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = 0; - - + return m_data->m_numAcceleratedShapes++; } - - -cl_mem b3GpuNarrowPhase::getBodiesGpu() +cl_mem b3GpuNarrowPhase::getBodiesGpu() { return (cl_mem)m_data->m_bodyBufferGPU->getBufferCL(); } @@ -743,25 +678,21 @@ const struct b3RigidBodyData* b3GpuNarrowPhase::getBodiesCpu() const return &m_data->m_bodyBufferCPU->at(0); }; - - - -int b3GpuNarrowPhase::getNumBodiesGpu() const +int b3GpuNarrowPhase::getNumBodiesGpu() const { return m_data->m_bodyBufferGPU->size(); } -cl_mem b3GpuNarrowPhase::getBodyInertiasGpu() +cl_mem b3GpuNarrowPhase::getBodyInertiasGpu() { return (cl_mem)m_data->m_inertiaBufferGPU->getBufferCL(); } -int b3GpuNarrowPhase::getNumBodyInertiasGpu() const +int b3GpuNarrowPhase::getNumBodyInertiasGpu() const { return m_data->m_inertiaBufferGPU->size(); } - b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex) { return m_data->m_collidablesCPU[collidableIndex]; @@ -789,25 +720,20 @@ const struct b3SapAabb* b3GpuNarrowPhase::getLocalSpaceAabbsCpu() const if (m_data->m_localShapeAABBCPU->size()) { return &m_data->m_localShapeAABBCPU->at(0); - } + } return 0; } - -cl_mem b3GpuNarrowPhase::getAabbLocalSpaceBufferGpu() +cl_mem b3GpuNarrowPhase::getAabbLocalSpaceBufferGpu() { return m_data->m_localShapeAABBGPU->getBufferCL(); } -int b3GpuNarrowPhase::getNumCollidablesGpu() const +int b3GpuNarrowPhase::getNumCollidablesGpu() const { return m_data->m_collidablesGPU->size(); } - - - - -int b3GpuNarrowPhase::getNumContactsGpu() const +int b3GpuNarrowPhase::getNumContactsGpu() const { return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); } @@ -824,37 +750,33 @@ const b3Contact4* b3GpuNarrowPhase::getContactsCPU() const void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects) { - cl_mem aabbsLocalSpace = m_data->m_localShapeAABBGPU->getBufferCL(); int nContactOut = 0; //swap buffer - m_data->m_currentContactBuffer=1-m_data->m_currentContactBuffer; + m_data->m_currentContactBuffer = 1 - m_data->m_currentContactBuffer; //int curSize = m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); int maxTriConvexPairCapacity = m_data->m_config.m_maxTriConvexPairCapacity; - int numTriConvexPairsOut=0; - - b3OpenCLArray<b3Int4> broadphasePairsGPU(m_context,m_queue); - broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs,numBroadphasePairs); - - + int numTriConvexPairsOut = 0; + b3OpenCLArray<b3Int4> broadphasePairsGPU(m_context, m_queue); + broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs, numBroadphasePairs); - b3OpenCLArray<b3Aabb> clAabbArrayWorldSpace(this->m_context,this->m_queue); - clAabbArrayWorldSpace.setFromOpenCLBuffer(aabbsWorldSpace,numObjects); + b3OpenCLArray<b3Aabb> clAabbArrayWorldSpace(this->m_context, this->m_queue); + clAabbArrayWorldSpace.setFromOpenCLBuffer(aabbsWorldSpace, numObjects); - b3OpenCLArray<b3Aabb> clAabbArrayLocalSpace(this->m_context,this->m_queue); - clAabbArrayLocalSpace.setFromOpenCLBuffer(aabbsLocalSpace,numObjects); + b3OpenCLArray<b3Aabb> clAabbArrayLocalSpace(this->m_context, this->m_queue); + clAabbArrayLocalSpace.setFromOpenCLBuffer(aabbsLocalSpace, numObjects); m_data->m_gpuSatCollision->computeConvexConvexContactsGPUSAT( &broadphasePairsGPU, numBroadphasePairs, m_data->m_bodyBufferGPU, m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer], nContactOut, - m_data->m_pBufContactBuffersGPU[1-m_data->m_currentContactBuffer], + m_data->m_pBufContactBuffersGPU[1 - m_data->m_currentContactBuffer], m_data->m_config.m_maxContactCapacity, m_data->m_config.m_compoundPairCapacity, *m_data->m_convexPolyhedraGPU, @@ -878,8 +800,7 @@ void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphase numObjects, maxTriConvexPairCapacity, *m_data->m_triangleConvexPairs, - numTriConvexPairsOut - ); + numTriConvexPairsOut); /*b3AlignedObjectArray<b3Int4> broadphasePairsCPU; broadphasePairsGPU.copyToHost(broadphasePairsCPU); @@ -892,105 +813,97 @@ const b3SapAabb& b3GpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const return m_data->m_localShapeAABBCPU->at(collidableIndex); } - - - - -int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation , const float* aabbMinPtr, const float* aabbMaxPtr,bool writeToGpu) +int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMinPtr, const float* aabbMaxPtr, bool writeToGpu) { - b3Vector3 aabbMin=b3MakeVector3(aabbMinPtr[0],aabbMinPtr[1],aabbMinPtr[2]); - b3Vector3 aabbMax=b3MakeVector3(aabbMaxPtr[0],aabbMaxPtr[1],aabbMaxPtr[2]); - + b3Vector3 aabbMin = b3MakeVector3(aabbMinPtr[0], aabbMinPtr[1], aabbMinPtr[2]); + b3Vector3 aabbMax = b3MakeVector3(aabbMaxPtr[0], aabbMaxPtr[1], aabbMaxPtr[2]); if (m_data->m_numAcceleratedRigidBodies >= (m_data->m_config.m_maxConvexBodies)) { - b3Error("registerRigidBody: exceeding the number of rigid bodies, %d > %d \n",m_data->m_numAcceleratedRigidBodies,m_data->m_config.m_maxConvexBodies); + b3Error("registerRigidBody: exceeding the number of rigid bodies, %d > %d \n", m_data->m_numAcceleratedRigidBodies, m_data->m_config.m_maxConvexBodies); return -1; } - - m_data->m_bodyBufferCPU->resize(m_data->m_numAcceleratedRigidBodies+1); - + + m_data->m_bodyBufferCPU->resize(m_data->m_numAcceleratedRigidBodies + 1); + b3RigidBodyData& body = m_data->m_bodyBufferCPU->at(m_data->m_numAcceleratedRigidBodies); - + float friction = 1.f; float restitution = 0.f; - + body.m_frictionCoeff = friction; body.m_restituitionCoeff = restitution; - body.m_angVel = b3MakeVector3(0,0,0); - body.m_linVel=b3MakeVector3(0,0,0);//.setZero(); - body.m_pos =b3MakeVector3(position[0],position[1],position[2]); - body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); + body.m_angVel = b3MakeVector3(0, 0, 0); + body.m_linVel = b3MakeVector3(0, 0, 0); //.setZero(); + body.m_pos = b3MakeVector3(position[0], position[1], position[2]); + body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]); body.m_collidableIdx = collidableIndex; - if (collidableIndex>=0) + if (collidableIndex >= 0) { -// body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType; - } else + // body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType; + } + else { - // body.m_shapeType = CollisionShape::SHAPE_PLANE; + // body.m_shapeType = CollisionShape::SHAPE_PLANE; m_planeBodyIndex = m_data->m_numAcceleratedRigidBodies; } //body.m_shapeType = shapeType; - - - body.m_invMass = mass? 1.f/mass : 0.f; - + + body.m_invMass = mass ? 1.f / mass : 0.f; + if (writeToGpu) { - m_data->m_bodyBufferGPU->copyFromHostPointer(&body,1,m_data->m_numAcceleratedRigidBodies); + m_data->m_bodyBufferGPU->copyFromHostPointer(&body, 1, m_data->m_numAcceleratedRigidBodies); } - + b3InertiaData& shapeInfo = m_data->m_inertiaBufferCPU->at(m_data->m_numAcceleratedRigidBodies); - - if (mass==0.f) + + if (mass == 0.f) { - if (m_data->m_numAcceleratedRigidBodies==0) + if (m_data->m_numAcceleratedRigidBodies == 0) m_static0Index = 0; - - shapeInfo.m_initInvInertia.setValue(0,0,0,0,0,0,0,0,0); - shapeInfo.m_invInertiaWorld.setValue(0,0,0,0,0,0,0,0,0); - } else + + shapeInfo.m_initInvInertia.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); + shapeInfo.m_invInertiaWorld.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0); + } + else { - - b3Assert(body.m_collidableIdx>=0); - + b3Assert(body.m_collidableIdx >= 0); + //approximate using the aabb of the shape - + //Aabb aabb = (*m_data->m_shapePointers)[shapeIndex]->m_aabb; - b3Vector3 halfExtents = (aabbMax-aabbMin);//*0.5f;//fake larger inertia makes demos more stable ;-) - + b3Vector3 halfExtents = (aabbMax - aabbMin); //*0.5f;//fake larger inertia makes demos more stable ;-) + b3Vector3 localInertia; - - float lx=2.f*halfExtents[0]; - float ly=2.f*halfExtents[1]; - float lz=2.f*halfExtents[2]; - - localInertia.setValue( (mass/12.0f) * (ly*ly + lz*lz), - (mass/12.0f) * (lx*lx + lz*lz), - (mass/12.0f) * (lx*lx + ly*ly)); - + + float lx = 2.f * halfExtents[0]; + float ly = 2.f * halfExtents[1]; + float lz = 2.f * halfExtents[2]; + + localInertia.setValue((mass / 12.0f) * (ly * ly + lz * lz), + (mass / 12.0f) * (lx * lx + lz * lz), + (mass / 12.0f) * (lx * lx + ly * ly)); + b3Vector3 invLocalInertia; - invLocalInertia[0] = 1.f/localInertia[0]; - invLocalInertia[1] = 1.f/localInertia[1]; - invLocalInertia[2] = 1.f/localInertia[2]; + invLocalInertia[0] = 1.f / localInertia[0]; + invLocalInertia[1] = 1.f / localInertia[1]; + invLocalInertia[2] = 1.f / localInertia[2]; invLocalInertia[3] = 0.f; - + shapeInfo.m_initInvInertia.setValue( - invLocalInertia[0], 0, 0, - 0, invLocalInertia[1], 0, - 0, 0, invLocalInertia[2]); + invLocalInertia[0], 0, 0, + 0, invLocalInertia[1], 0, + 0, 0, invLocalInertia[2]); - b3Matrix3x3 m (body.m_quat); + b3Matrix3x3 m(body.m_quat); shapeInfo.m_invInertiaWorld = m.scaled(invLocalInertia) * m.transpose(); - } - + if (writeToGpu) - m_data->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo,1,m_data->m_numAcceleratedRigidBodies); - - - + m_data->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo, 1, m_data->m_numAcceleratedRigidBodies); + return m_data->m_numAcceleratedRigidBodies++; } @@ -999,15 +912,13 @@ int b3GpuNarrowPhase::getNumRigidBodies() const return m_data->m_numAcceleratedRigidBodies; } -void b3GpuNarrowPhase::writeAllBodiesToGpu() +void b3GpuNarrowPhase::writeAllBodiesToGpu() { - if (m_data->m_localShapeAABBCPU->size()) { m_data->m_localShapeAABBGPU->copyFromHost(*m_data->m_localShapeAABBCPU); } - - + m_data->m_gpuChildShapes->copyFromHost(m_data->m_cpuChildShapes); m_data->m_convexFacesGPU->copyFromHost(m_data->m_convexFaces); m_data->m_convexPolyhedraGPU->copyFromHost(m_data->m_convexPolyhedra); @@ -1018,25 +929,21 @@ void b3GpuNarrowPhase::writeAllBodiesToGpu() m_data->m_treeNodesGPU->copyFromHost(m_data->m_treeNodesCPU); m_data->m_subTreesGPU->copyFromHost(m_data->m_subTreesCPU); - m_data->m_bodyBufferGPU->resize(m_data->m_numAcceleratedRigidBodies); m_data->m_inertiaBufferGPU->resize(m_data->m_numAcceleratedRigidBodies); - + if (m_data->m_numAcceleratedRigidBodies) { - m_data->m_bodyBufferGPU->copyFromHostPointer(&m_data->m_bodyBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); - m_data->m_inertiaBufferGPU->copyFromHostPointer(&m_data->m_inertiaBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); + m_data->m_bodyBufferGPU->copyFromHostPointer(&m_data->m_bodyBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies); + m_data->m_inertiaBufferGPU->copyFromHostPointer(&m_data->m_inertiaBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies); } - if (m_data->m_collidablesCPU.size()) + if (m_data->m_collidablesCPU.size()) { m_data->m_collidablesGPU->copyFromHost(m_data->m_collidablesCPU); } - - } - -void b3GpuNarrowPhase::reset() +void b3GpuNarrowPhase::reset() { m_data->m_numAcceleratedShapes = 0; m_data->m_numAcceleratedRigidBodies = 0; @@ -1053,21 +960,19 @@ void b3GpuNarrowPhase::reset() m_data->m_treeNodesCPU.resize(0); m_data->m_subTreesCPU.resize(0); m_data->m_bvhInfoCPU.resize(0); - } - -void b3GpuNarrowPhase::readbackAllBodiesToCpu() +void b3GpuNarrowPhase::readbackAllBodiesToCpu() { - m_data->m_bodyBufferGPU->copyToHostPointer(&m_data->m_bodyBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); + m_data->m_bodyBufferGPU->copyToHostPointer(&m_data->m_bodyBufferCPU->at(0), m_data->m_numAcceleratedRigidBodies); } -void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation , int bodyIndex) +void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation, int bodyIndex) { - if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) + if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size()) { - m_data->m_bodyBufferCPU->at(bodyIndex).m_pos=b3MakeVector3(position[0],position[1],position[2]); - m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); + m_data->m_bodyBufferCPU->at(bodyIndex).m_pos = b3MakeVector3(position[0], position[1], position[2]); + m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]); } else { @@ -1076,24 +981,25 @@ void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation } void b3GpuNarrowPhase::setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex) { - if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) + if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size()) { - m_data->m_bodyBufferCPU->at(bodyIndex).m_linVel=b3MakeVector3(linVel[0],linVel[1],linVel[2]); - m_data->m_bodyBufferCPU->at(bodyIndex).m_angVel=b3MakeVector3(angVel[0],angVel[1],angVel[2]); - } else + m_data->m_bodyBufferCPU->at(bodyIndex).m_linVel = b3MakeVector3(linVel[0], linVel[1], linVel[2]); + m_data->m_bodyBufferCPU->at(bodyIndex).m_angVel = b3MakeVector3(angVel[0], angVel[1], angVel[2]); + } + else { b3Warning("setObjectVelocityCpu out of range.\n"); } } -bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const +bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const { - if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size()) + if (bodyIndex >= 0 && bodyIndex < m_data->m_bodyBufferCPU->size()) { position[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.x; position[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.y; position[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.z; - position[3] = 1.f;//or 1 + position[3] = 1.f; //or 1 orientation[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.x; orientation[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.y; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h index 05ff3fd09e..21a68de343 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h @@ -9,11 +9,10 @@ class b3GpuNarrowPhase { protected: - - struct b3GpuNarrowPhaseInternalData* m_data; + struct b3GpuNarrowPhaseInternalData* m_data; int m_acceleratedCompanionShapeIndex; int m_planeBodyIndex; - int m_static0Index; + int m_static0Index; cl_context m_context; cl_device_id m_device; @@ -23,64 +22,58 @@ protected: int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling); public: - - - - b3GpuNarrowPhase(cl_context vtx, cl_device_id dev, cl_command_queue q, const struct b3Config& config); virtual ~b3GpuNarrowPhase(void); - int registerSphereShape(float radius); - int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant); + int registerSphereShape(float radius); + int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant); int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes); int registerFace(const b3Vector3& faceNormal, float faceConstant); - - int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling); - + + int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling); + //do they need to be merged? - - int registerConvexHullShape(b3ConvexUtility* utilPtr); - int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling); - int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu); - void setObjectTransform(const float* position, const float* orientation , int bodyIndex); + int registerConvexHullShape(b3ConvexUtility* utilPtr); + int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling); + + int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax, bool writeToGpu); + void setObjectTransform(const float* position, const float* orientation, int bodyIndex); - void writeAllBodiesToGpu(); - void reset(); - void readbackAllBodiesToCpu(); - bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const; + void writeAllBodiesToGpu(); + void reset(); + void readbackAllBodiesToCpu(); + bool getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const; - void setObjectTransformCpu(float* position, float* orientation , int bodyIndex); + void setObjectTransformCpu(float* position, float* orientation, int bodyIndex); void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex); - virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects); - - cl_mem getBodiesGpu(); + cl_mem getBodiesGpu(); const struct b3RigidBodyData* getBodiesCpu() const; //struct b3RigidBodyData* getBodiesCpu(); - int getNumBodiesGpu() const; + int getNumBodiesGpu() const; - cl_mem getBodyInertiasGpu(); - int getNumBodyInertiasGpu() const; + cl_mem getBodyInertiasGpu(); + int getNumBodyInertiasGpu() const; - cl_mem getCollidablesGpu(); + cl_mem getCollidablesGpu(); const struct b3Collidable* getCollidablesCpu() const; - int getNumCollidablesGpu() const; + int getNumCollidablesGpu() const; const struct b3SapAabb* getLocalSpaceAabbsCpu() const; const struct b3Contact4* getContactsCPU() const; - cl_mem getContactsGpu(); - int getNumContactsGpu() const; + cl_mem getContactsGpu(); + int getNumContactsGpu() const; + + cl_mem getAabbLocalSpaceBufferGpu(); - cl_mem getAabbLocalSpaceBufferGpu(); - int getNumRigidBodies() const; int allocateCollidable(); @@ -92,18 +85,17 @@ public: b3Collidable& getCollidableCpu(int collidableIndex); const b3Collidable& getCollidableCpu(int collidableIndex) const; - const b3GpuNarrowPhaseInternalData* getInternalData() const + const b3GpuNarrowPhaseInternalData* getInternalData() const { - return m_data; + return m_data; } - b3GpuNarrowPhaseInternalData* getInternalData() + b3GpuNarrowPhaseInternalData* getInternalData() { - return m_data; + return m_data; } const struct b3SapAabb& getLocalSpaceAabb(int collidableIndex) const; }; -#endif //B3_GPU_NARROWPHASE_H - +#endif //B3_GPU_NARROWPHASE_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h index 8a7f1ea859..716a5ea0fc 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h @@ -20,57 +20,53 @@ #include "Bullet3Common/shared/b3Int4.h" #include "Bullet3Common/shared/b3Int2.h" - class b3ConvexUtility; struct b3GpuNarrowPhaseInternalData { b3AlignedObjectArray<b3ConvexUtility*>* m_convexData; - + b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra; b3AlignedObjectArray<b3Vector3> m_uniqueEdges; b3AlignedObjectArray<b3Vector3> m_convexVertices; b3AlignedObjectArray<int> m_convexIndices; - + b3OpenCLArray<b3ConvexPolyhedronData>* m_convexPolyhedraGPU; b3OpenCLArray<b3Vector3>* m_uniqueEdgesGPU; b3OpenCLArray<b3Vector3>* m_convexVerticesGPU; b3OpenCLArray<int>* m_convexIndicesGPU; - - b3OpenCLArray<b3Vector3>* m_worldVertsB1GPU; - b3OpenCLArray<b3Int4>* m_clippingFacesOutGPU; - b3OpenCLArray<b3Vector3>* m_worldNormalsAGPU; - b3OpenCLArray<b3Vector3>* m_worldVertsA1GPU; - b3OpenCLArray<b3Vector3>* m_worldVertsB2GPU; - + + b3OpenCLArray<b3Vector3>* m_worldVertsB1GPU; + b3OpenCLArray<b3Int4>* m_clippingFacesOutGPU; + b3OpenCLArray<b3Vector3>* m_worldNormalsAGPU; + b3OpenCLArray<b3Vector3>* m_worldVertsA1GPU; + b3OpenCLArray<b3Vector3>* m_worldVertsB2GPU; + b3AlignedObjectArray<b3GpuChildShape> m_cpuChildShapes; - b3OpenCLArray<b3GpuChildShape>* m_gpuChildShapes; - + b3OpenCLArray<b3GpuChildShape>* m_gpuChildShapes; + b3AlignedObjectArray<b3GpuFace> m_convexFaces; b3OpenCLArray<b3GpuFace>* m_convexFacesGPU; - - struct GpuSatCollision* m_gpuSatCollision; - - - b3OpenCLArray<b3Int4>* m_triangleConvexPairs; - - + + struct GpuSatCollision* m_gpuSatCollision; + + b3OpenCLArray<b3Int4>* m_triangleConvexPairs; + b3OpenCLArray<b3Contact4>* m_pBufContactBuffersGPU[2]; - int m_currentContactBuffer; + int m_currentContactBuffer; b3AlignedObjectArray<b3Contact4>* m_pBufContactOutCPU; - - + b3AlignedObjectArray<b3RigidBodyData>* m_bodyBufferCPU; b3OpenCLArray<b3RigidBodyData>* m_bodyBufferGPU; - - b3AlignedObjectArray<b3InertiaData>* m_inertiaBufferCPU; - b3OpenCLArray<b3InertiaData>* m_inertiaBufferGPU; - + + b3AlignedObjectArray<b3InertiaData>* m_inertiaBufferCPU; + b3OpenCLArray<b3InertiaData>* m_inertiaBufferGPU; + int m_numAcceleratedShapes; int m_numAcceleratedRigidBodies; - - b3AlignedObjectArray<b3Collidable> m_collidablesCPU; - b3OpenCLArray<b3Collidable>* m_collidablesGPU; + + b3AlignedObjectArray<b3Collidable> m_collidablesCPU; + b3OpenCLArray<b3Collidable>* m_collidablesGPU; b3OpenCLArray<b3SapAabb>* m_localShapeAABBGPU; b3AlignedObjectArray<b3SapAabb>* m_localShapeAABBCPU; @@ -78,18 +74,16 @@ struct b3GpuNarrowPhaseInternalData b3AlignedObjectArray<class b3OptimizedBvh*> m_bvhData; b3AlignedObjectArray<class b3TriangleIndexVertexArray*> m_meshInterfaces; - b3AlignedObjectArray<b3QuantizedBvhNode> m_treeNodesCPU; - b3AlignedObjectArray<b3BvhSubtreeInfo> m_subTreesCPU; + b3AlignedObjectArray<b3QuantizedBvhNode> m_treeNodesCPU; + b3AlignedObjectArray<b3BvhSubtreeInfo> m_subTreesCPU; + + b3AlignedObjectArray<b3BvhInfo> m_bvhInfoCPU; + b3OpenCLArray<b3BvhInfo>* m_bvhInfoGPU; - b3AlignedObjectArray<b3BvhInfo> m_bvhInfoCPU; - b3OpenCLArray<b3BvhInfo>* m_bvhInfoGPU; - - b3OpenCLArray<b3QuantizedBvhNode>* m_treeNodesGPU; - b3OpenCLArray<b3BvhSubtreeInfo>* m_subTreesGPU; - + b3OpenCLArray<b3QuantizedBvhNode>* m_treeNodesGPU; + b3OpenCLArray<b3BvhSubtreeInfo>* m_subTreesGPU; - b3Config m_config; - + b3Config m_config; }; -#endif //B3_GPU_NARROWPHASE_INTERNAL_DATA_H +#endif //B3_GPU_NARROWPHASE_INTERNAL_DATA_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp index 0d3d50c548..bd9d6bb04b 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp @@ -14,11 +14,10 @@ subject to the following restrictions: */ //Originally written by Erwin Coumans - bool useGpuInitSolverBodies = true; bool useGpuInfo1 = true; -bool useGpuInfo2= true; -bool useGpuSolveJointConstraintRows=true; +bool useGpuInfo2 = true; +bool useGpuSolveJointConstraintRows = true; bool useGpuWriteBackVelocities = true; bool gpuBreakConstraints = true; @@ -29,27 +28,25 @@ bool gpuBreakConstraints = true; #include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" #include <new> #include "Bullet3Common/b3AlignedObjectArray.h" -#include <string.h> //for memset +#include <string.h> //for memset #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" #include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" -#include "Bullet3OpenCL/RigidBody/kernels/jointSolver.h" //solveConstraintRowsCL +#include "Bullet3OpenCL/RigidBody/kernels/jointSolver.h" //solveConstraintRowsCL #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" #define B3_JOINT_SOLVER_PATH "src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl" - struct b3GpuPgsJacobiSolverInternalData { - cl_context m_context; cl_device_id m_device; cl_command_queue m_queue; - b3PrefixScanCL* m_prefixScan; + b3PrefixScanCL* m_prefixScan; cl_kernel m_solveJointConstraintRowsKernels; cl_kernel m_initSolverBodiesKernel; @@ -59,31 +56,27 @@ struct b3GpuPgsJacobiSolverInternalData cl_kernel m_writeBackVelocitiesKernel; cl_kernel m_breakViolatedConstraintsKernel; - b3OpenCLArray<unsigned int>* m_gpuConstraintRowOffsets; + b3OpenCLArray<unsigned int>* m_gpuConstraintRowOffsets; - b3OpenCLArray<b3GpuSolverBody>* m_gpuSolverBodies; - b3OpenCLArray<b3BatchConstraint>* m_gpuBatchConstraints; - b3OpenCLArray<b3GpuSolverConstraint>* m_gpuConstraintRows; - b3OpenCLArray<unsigned int>* m_gpuConstraintInfo1; + b3OpenCLArray<b3GpuSolverBody>* m_gpuSolverBodies; + b3OpenCLArray<b3BatchConstraint>* m_gpuBatchConstraints; + b3OpenCLArray<b3GpuSolverConstraint>* m_gpuConstraintRows; + b3OpenCLArray<unsigned int>* m_gpuConstraintInfo1; -// b3AlignedObjectArray<b3GpuSolverBody> m_cpuSolverBodies; - b3AlignedObjectArray<b3BatchConstraint> m_cpuBatchConstraints; - b3AlignedObjectArray<b3GpuSolverConstraint> m_cpuConstraintRows; - b3AlignedObjectArray<unsigned int> m_cpuConstraintInfo1; - b3AlignedObjectArray<unsigned int> m_cpuConstraintRowOffsets; + // b3AlignedObjectArray<b3GpuSolverBody> m_cpuSolverBodies; + b3AlignedObjectArray<b3BatchConstraint> m_cpuBatchConstraints; + b3AlignedObjectArray<b3GpuSolverConstraint> m_cpuConstraintRows; + b3AlignedObjectArray<unsigned int> m_cpuConstraintInfo1; + b3AlignedObjectArray<unsigned int> m_cpuConstraintRowOffsets; - b3AlignedObjectArray<b3RigidBodyData> m_cpuBodies; - b3AlignedObjectArray<b3InertiaData> m_cpuInertias; + b3AlignedObjectArray<b3RigidBodyData> m_cpuBodies; + b3AlignedObjectArray<b3InertiaData> m_cpuInertias; - b3AlignedObjectArray<b3GpuGenericConstraint> m_cpuConstraints; - b3AlignedObjectArray<int> m_batchSizes; - - + b3AlignedObjectArray<int> m_batchSizes; }; - /* static b3Transform getWorldTransform(b3RigidBodyData* rb) { @@ -100,12 +93,12 @@ static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) */ -static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) +static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) { return rb->m_linVel; } -static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) +static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) { return rb->m_angVel; } @@ -114,12 +107,9 @@ b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos) { //we also calculate lin/ang velocity for kinematic objects return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos); - } - - -b3GpuPgsConstraintSolver::b3GpuPgsConstraintSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs) +b3GpuPgsConstraintSolver::b3GpuPgsConstraintSolver(cl_context ctx, cl_device_id device, cl_command_queue queue, bool usePgs) { m_usePgs = usePgs; m_gpuData = new b3GpuPgsJacobiSolverInternalData(); @@ -127,45 +117,40 @@ b3GpuPgsConstraintSolver::b3GpuPgsConstraintSolver (cl_context ctx, cl_device_id m_gpuData->m_device = device; m_gpuData->m_queue = queue; - m_gpuData->m_prefixScan = new b3PrefixScanCL(ctx,device,queue); + m_gpuData->m_prefixScan = new b3PrefixScanCL(ctx, device, queue); - m_gpuData->m_gpuConstraintRowOffsets = new b3OpenCLArray<unsigned int>(m_gpuData->m_context,m_gpuData->m_queue); + m_gpuData->m_gpuConstraintRowOffsets = new b3OpenCLArray<unsigned int>(m_gpuData->m_context, m_gpuData->m_queue); - m_gpuData->m_gpuSolverBodies = new b3OpenCLArray<b3GpuSolverBody>(m_gpuData->m_context,m_gpuData->m_queue); - m_gpuData->m_gpuBatchConstraints = new b3OpenCLArray<b3BatchConstraint>(m_gpuData->m_context,m_gpuData->m_queue); - m_gpuData->m_gpuConstraintRows = new b3OpenCLArray<b3GpuSolverConstraint>(m_gpuData->m_context,m_gpuData->m_queue); - m_gpuData->m_gpuConstraintInfo1 = new b3OpenCLArray<unsigned int>(m_gpuData->m_context,m_gpuData->m_queue); - cl_int errNum=0; + m_gpuData->m_gpuSolverBodies = new b3OpenCLArray<b3GpuSolverBody>(m_gpuData->m_context, m_gpuData->m_queue); + m_gpuData->m_gpuBatchConstraints = new b3OpenCLArray<b3BatchConstraint>(m_gpuData->m_context, m_gpuData->m_queue); + m_gpuData->m_gpuConstraintRows = new b3OpenCLArray<b3GpuSolverConstraint>(m_gpuData->m_context, m_gpuData->m_queue); + m_gpuData->m_gpuConstraintInfo1 = new b3OpenCLArray<unsigned int>(m_gpuData->m_context, m_gpuData->m_queue); + cl_int errNum = 0; { - cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,&errNum,"",B3_JOINT_SOLVER_PATH); + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, &errNum, "", B3_JOINT_SOLVER_PATH); //cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context,m_gpuData->m_device,0,&errNum,"",B3_JOINT_SOLVER_PATH,true); - b3Assert(errNum==CL_SUCCESS); - m_gpuData->m_solveJointConstraintRowsKernels = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device,solveConstraintRowsCL, "solveJointConstraintRows",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); - m_gpuData->m_initSolverBodiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"initSolverBodies",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); - m_gpuData->m_getInfo1Kernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"getInfo1Kernel",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); - m_gpuData->m_initBatchConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"initBatchConstraintsKernel",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); - m_gpuData->m_getInfo2Kernel= b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"getInfo2Kernel",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); - m_gpuData->m_writeBackVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"writeBackVelocitiesKernel",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); - m_gpuData->m_breakViolatedConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context,m_gpuData->m_device,solveConstraintRowsCL,"breakViolatedConstraintsKernel",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); - - - + b3Assert(errNum == CL_SUCCESS); + m_gpuData->m_solveJointConstraintRowsKernels = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "solveJointConstraintRows", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); + m_gpuData->m_initSolverBodiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "initSolverBodies", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); + m_gpuData->m_getInfo1Kernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "getInfo1Kernel", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); + m_gpuData->m_initBatchConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "initBatchConstraintsKernel", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); + m_gpuData->m_getInfo2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "getInfo2Kernel", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); + m_gpuData->m_writeBackVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "writeBackVelocitiesKernel", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); + m_gpuData->m_breakViolatedConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "breakViolatedConstraintsKernel", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); clReleaseProgram(prog); } - - } -b3GpuPgsConstraintSolver::~b3GpuPgsConstraintSolver () +b3GpuPgsConstraintSolver::~b3GpuPgsConstraintSolver() { clReleaseKernel(m_gpuData->m_solveJointConstraintRowsKernels); clReleaseKernel(m_gpuData->m_initSolverBodiesKernel); @@ -195,16 +180,12 @@ struct b3BatchConstraint static b3AlignedObjectArray<b3BatchConstraint> batchConstraints; - -void b3GpuPgsConstraintSolver::recomputeBatches() +void b3GpuPgsConstraintSolver::recomputeBatches() { m_gpuData->m_batchSizes.clear(); } - - - -b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("GPU solveGroupCacheFriendlySetup"); batchConstraints.resize(numConstraints); @@ -212,7 +193,6 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 m_staticIdx = -1; m_maxOverrideNumSolverIterations = 0; - /* m_gpuData->m_gpuBodies->resize(numBodies); m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies); @@ -223,15 +203,13 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 m_gpuData->m_gpuSolverBodies->resize(numBodies); - m_tmpSolverBodyPool.resize(numBodies); { - if (useGpuInitSolverBodies) { B3_PROFILE("m_initSolverBodiesKernel"); - b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_initSolverBodiesKernel,"m_initSolverBodiesKernel"); + b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_initSolverBodiesKernel, "m_initSolverBodiesKernel"); launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); launcher.setBuffer(gpuBodies->getBufferCL()); launcher.setConst(numBodies); @@ -239,48 +217,44 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 clFinish(m_gpuData->m_queue); // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); - } else + } + else { gpuBodies->copyToHost(m_gpuData->m_cpuBodies); - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { - b3RigidBodyData& body = m_gpuData->m_cpuBodies[i]; b3GpuSolverBody& solverBody = m_tmpSolverBodyPool[i]; - initSolverBody(i,&solverBody,&body); + initSolverBody(i, &solverBody, &body); solverBody.m_originalBodyIndex = i; } m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); } } -// int totalBodies = 0; + // int totalBodies = 0; int totalNumRows = 0; //b3RigidBody* rb0=0,*rb1=0; //if (1) { { - - // int i; m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); // b3OpenCLArray<b3GpuGenericConstraint> gpuConstraints(m_gpuData->m_context,m_gpuData->m_queue); - if (useGpuInfo1) { B3_PROFILE("info1 and init batchConstraint"); - + m_gpuData->m_gpuConstraintInfo1->resize(numConstraints); - if (1) { B3_PROFILE("getInfo1Kernel"); - b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo1Kernel,"m_getInfo1Kernel"); + b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_getInfo1Kernel, "m_getInfo1Kernel"); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(gpuConstraints->getBufferCL()); launcher.setConst(numConstraints); @@ -288,19 +262,19 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 clFinish(m_gpuData->m_queue); } - if (m_gpuData->m_batchSizes.size()==0) + if (m_gpuData->m_batchSizes.size() == 0) { B3_PROFILE("initBatchConstraintsKernel"); m_gpuData->m_gpuConstraintRowOffsets->resize(numConstraints); - unsigned int total=0; - m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1,*m_gpuData->m_gpuConstraintRowOffsets,numConstraints,&total); - unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints-1); - totalNumRows = total+lastElem; + unsigned int total = 0; + m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1, *m_gpuData->m_gpuConstraintRowOffsets, numConstraints, &total); + unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints - 1); + totalNumRows = total + lastElem; { B3_PROFILE("init batch constraints"); - b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_initBatchConstraintsKernel,"m_initBatchConstraintsKernel"); + b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_initBatchConstraintsKernel, "m_initBatchConstraintsKernel"); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); @@ -313,79 +287,74 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 //assume the batching happens on CPU, so copy the data m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); } - } + } else { - totalNumRows = 0; + totalNumRows = 0; gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); //calculate the total number of contraint rows - for (int i=0;i<numConstraints;i++) + for (int i = 0; i < numConstraints; i++) { - unsigned int& info1= m_tmpConstraintSizesPool[i]; + unsigned int& info1 = m_tmpConstraintSizesPool[i]; // unsigned int info1; if (m_gpuData->m_cpuConstraints[i].isEnabled()) { - - m_gpuData->m_cpuConstraints[i].getInfo1(&info1,&m_gpuData->m_cpuBodies[0]); - } else + m_gpuData->m_cpuConstraints[i].getInfo1(&info1, &m_gpuData->m_cpuBodies[0]); + } + else { info1 = 0; } - + totalNumRows += info1; } m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool); - } m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); m_gpuData->m_gpuConstraintRows->resize(totalNumRows); - + // b3GpuConstraintArray verify; if (useGpuInfo2) { { - B3_PROFILE("getInfo2Kernel"); - b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_getInfo2Kernel,"m_getInfo2Kernel"); - launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); - launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); - launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); - launcher.setBuffer(gpuConstraints->getBufferCL()); - launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); - launcher.setBuffer(gpuBodies->getBufferCL()); - launcher.setBuffer(gpuInertias->getBufferCL()); - launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); - launcher.setConst(infoGlobal.m_timeStep); - launcher.setConst(infoGlobal.m_erp); - launcher.setConst(infoGlobal.m_globalCfm); - launcher.setConst(infoGlobal.m_damping); - launcher.setConst(infoGlobal.m_numIterations); - launcher.setConst(numConstraints); - launcher.launch1D(numConstraints); - clFinish(m_gpuData->m_queue); - - if (m_gpuData->m_batchSizes.size()==0) - m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); - //m_gpuData->m_gpuConstraintRows->copyToHost(verify); - //m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool); - - + B3_PROFILE("getInfo2Kernel"); + b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_getInfo2Kernel, "m_getInfo2Kernel"); + launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setBuffer(gpuInertias->getBufferCL()); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setConst(infoGlobal.m_timeStep); + launcher.setConst(infoGlobal.m_erp); + launcher.setConst(infoGlobal.m_globalCfm); + launcher.setConst(infoGlobal.m_damping); + launcher.setConst(infoGlobal.m_numIterations); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + clFinish(m_gpuData->m_queue); - } - } + if (m_gpuData->m_batchSizes.size() == 0) + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + //m_gpuData->m_gpuConstraintRows->copyToHost(verify); + //m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool); + } + } else { - gpuInertias->copyToHost(m_gpuData->m_cpuInertias); - ///setup the b3SolverConstraints - - for (int i=0;i<numConstraints;i++) + ///setup the b3SolverConstraints + + for (int i = 0; i < numConstraints; i++) { const int& info1 = m_tmpConstraintSizesPool[i]; - + if (info1) { int constraintIndex = batchConstraints[i].m_originalConstraintIndex; @@ -394,15 +363,13 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[constraintRowOffset]; b3GpuGenericConstraint& constraint = m_gpuData->m_cpuConstraints[i]; - b3RigidBodyData& rbA = m_gpuData->m_cpuBodies[ constraint.getRigidBodyA()]; + b3RigidBodyData& rbA = m_gpuData->m_cpuBodies[constraint.getRigidBodyA()]; //b3RigidBody& rbA = constraint.getRigidBodyA(); - // b3RigidBody& rbB = constraint.getRigidBodyB(); - b3RigidBodyData& rbB = m_gpuData->m_cpuBodies[ constraint.getRigidBodyB()]; - - + // b3RigidBody& rbB = constraint.getRigidBodyB(); + b3RigidBodyData& rbB = m_gpuData->m_cpuBodies[constraint.getRigidBodyB()]; - int solverBodyIdA = constraint.getRigidBodyA();//getOrInitSolverBody(constraint.getRigidBodyA(),bodies,inertias); - int solverBodyIdB = constraint.getRigidBodyB();//getOrInitSolverBody(constraint.getRigidBodyB(),bodies,inertias); + int solverBodyIdA = constraint.getRigidBodyA(); //getOrInitSolverBody(constraint.getRigidBodyA(),bodies,inertias); + int solverBodyIdB = constraint.getRigidBodyB(); //getOrInitSolverBody(constraint.getRigidBodyB(),bodies,inertias); b3GpuSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; b3GpuSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; @@ -410,7 +377,8 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 if (rbA.m_invMass) { batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA; - } else + } + else { if (!solverBodyIdA) m_staticIdx = 0; @@ -420,29 +388,28 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 if (rbB.m_invMass) { batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; - } else + } + else { if (!solverBodyIdB) m_staticIdx = 0; batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; } - - int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + int overrideNumSolverIterations = 0; //constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations) m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - int j; - for ( j=0;j<info1;j++) + for (j = 0; j < info1; j++) { - memset(¤tConstraintRow[j],0,sizeof(b3GpuSolverConstraint)); - currentConstraintRow[j].m_angularComponentA.setValue(0,0,0); - currentConstraintRow[j].m_angularComponentB.setValue(0,0,0); + memset(¤tConstraintRow[j], 0, sizeof(b3GpuSolverConstraint)); + currentConstraintRow[j].m_angularComponentA.setValue(0, 0, 0); + currentConstraintRow[j].m_angularComponentB.setValue(0, 0, 0); currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f; currentConstraintRow[j].m_cfm = 0.f; - currentConstraintRow[j].m_contactNormal.setValue(0,0,0); + currentConstraintRow[j].m_contactNormal.setValue(0, 0, 0); currentConstraintRow[j].m_friction = 0.f; currentConstraintRow[j].m_frictionIndex = 0; currentConstraintRow[j].m_jacDiagABInv = 0.f; @@ -451,13 +418,13 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 currentConstraintRow[j].m_originalContactPoint = 0; currentConstraintRow[j].m_overrideNumSolverIterations = 0; - currentConstraintRow[j].m_relpos1CrossNormal.setValue(0,0,0); - currentConstraintRow[j].m_relpos2CrossNormal.setValue(0,0,0); + currentConstraintRow[j].m_relpos1CrossNormal.setValue(0, 0, 0); + currentConstraintRow[j].m_relpos2CrossNormal.setValue(0, 0, 0); currentConstraintRow[j].m_rhs = 0.f; currentConstraintRow[j].m_rhsPenetration = 0.f; currentConstraintRow[j].m_solverBodyIdA = 0; currentConstraintRow[j].m_solverBodyIdB = 0; - + currentConstraintRow[j].m_lowerLimit = -B3_INFINITY; currentConstraintRow[j].m_upperLimit = B3_INFINITY; currentConstraintRow[j].m_appliedImpulse = 0.f; @@ -467,26 +434,25 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; } - bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); - bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - + bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); + bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); + bodyAPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + bodyAPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); + bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); + bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); + bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); b3GpuConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; + info2.fps = 1.f / infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; info2.m_J2linearAxis = 0; info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(b3GpuSolverConstraint)/sizeof(b3Scalar);//check this + info2.rowskip = sizeof(b3GpuSolverConstraint) / sizeof(b3Scalar); //check this ///the size of b3GpuSolverConstraint needs be a multiple of b3Scalar - b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3GpuSolverConstraint)); + b3Assert(info2.rowskip * sizeof(b3Scalar) == sizeof(b3GpuSolverConstraint)); info2.m_constraintError = ¤tConstraintRow->m_rhs; currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; info2.m_damping = infoGlobal.m_damping; @@ -494,47 +460,45 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; info2.m_numIterations = infoGlobal.m_numIterations; - m_gpuData->m_cpuConstraints[i].getInfo2(&info2,&m_gpuData->m_cpuBodies[0]); + m_gpuData->m_cpuConstraints[i].getInfo2(&info2, &m_gpuData->m_cpuBodies[0]); ///finalize the constraint setup - for ( j=0;j<info1;j++) + for (j = 0; j < info1; j++) { b3GpuSolverConstraint& solverConstraint = currentConstraintRow[j]; - if (solverConstraint.m_upperLimit>=m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold()) + if (solverConstraint.m_upperLimit >= m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold()) { solverConstraint.m_upperLimit = m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold(); } - if (solverConstraint.m_lowerLimit<=-m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold()) + if (solverConstraint.m_lowerLimit <= -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold()) { solverConstraint.m_lowerLimit = -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold(); } - // solverConstraint.m_originalContactPoint = constraint; - - b3Matrix3x3& invInertiaWorldA= m_gpuData->m_cpuInertias[constraint.getRigidBodyA()].m_invInertiaWorld; - { + // solverConstraint.m_originalContactPoint = constraint; + b3Matrix3x3& invInertiaWorldA = m_gpuData->m_cpuInertias[constraint.getRigidBodyA()].m_invInertiaWorld; + { //b3Vector3 angularFactorA(1,1,1); const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA; + solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1; //*angularFactorA; } - - b3Matrix3x3& invInertiaWorldB= m_gpuData->m_cpuInertias[constraint.getRigidBodyB()].m_invInertiaWorld; - { + b3Matrix3x3& invInertiaWorldB = m_gpuData->m_cpuInertias[constraint.getRigidBodyB()].m_invInertiaWorld; + { const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = invInertiaWorldB*ftorqueAxis2;//*constraint.getRigidBodyB().getAngularFactor(); + solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2; //*constraint.getRigidBodyB().getAngularFactor(); } { //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal //because it gets multiplied iMJlB - b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.m_invMass; - b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal; - b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.m_invMass;//sign of normal? - b3Vector3 iMJaB = invInertiaWorldB*solverConstraint.m_relpos2CrossNormal; + b3Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.m_invMass; + b3Vector3 iMJaA = invInertiaWorldA * solverConstraint.m_relpos1CrossNormal; + b3Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.m_invMass; //sign of normal? + b3Vector3 iMJaB = invInertiaWorldB * solverConstraint.m_relpos2CrossNormal; b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); @@ -542,10 +506,9 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); b3Scalar fsum = b3Fabs(sum); b3Assert(fsum > B3_EPSILON); - solverConstraint.m_jacDiagABInv = fsum>B3_EPSILON?b3Scalar(1.)/sum : 0.f; + solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f; } - ///fix rhs ///todo: add force/torque accelerators { @@ -553,94 +516,80 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3 b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel); b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; b3Scalar restitution = 0.f; - b3Scalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - b3Scalar velocityError = restitution - rel_vel * info2.m_damping; - b3Scalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - b3Scalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + b3Scalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2 + b3Scalar velocityError = restitution - rel_vel * info2.m_damping; + b3Scalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_appliedImpulse = 0.f; - } } - } } - - m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool); m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool); - if (m_gpuData->m_batchSizes.size()==0) + if (m_gpuData->m_batchSizes.size() == 0) m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); else m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); - - - }//end useGpuInfo2 - - + } //end useGpuInfo2 } #ifdef B3_SUPPORT_CONTACT_CONSTRAINTS { int i; - for (i=0;i<numManifolds;i++) + for (i = 0; i < numManifolds; i++) { b3Contact4& manifold = manifoldPtr[i]; - convertContact(bodies,inertias,&manifold,infoGlobal); + convertContact(bodies, inertias, &manifold, infoGlobal); } } -#endif //B3_SUPPORT_CONTACT_CONSTRAINTS +#endif //B3_SUPPORT_CONTACT_CONSTRAINTS } -// b3ContactSolverInfo info = infoGlobal; - - -// int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); -// int numConstraintPool = m_tmpSolverContactConstraintPool.size(); -// int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); + // b3ContactSolverInfo info = infoGlobal; + // int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); + // int numConstraintPool = m_tmpSolverContactConstraintPool.size(); + // int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); return 0.f; - } - - ///a straight copy from GPU/OpenCL kernel, for debugging -__inline void internalApplyImpulse( b3GpuSolverBody* body, const b3Vector3& linearComponent, const b3Vector3& angularComponent,float impulseMagnitude) +__inline void internalApplyImpulse(b3GpuSolverBody* body, const b3Vector3& linearComponent, const b3Vector3& angularComponent, float impulseMagnitude) { - body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor; - body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor); + body->m_deltaLinearVelocity += linearComponent * impulseMagnitude * body->m_linearFactor; + body->m_deltaAngularVelocity += angularComponent * (impulseMagnitude * body->m_angularFactor); } - -void resolveSingleConstraintRowGeneric2( b3GpuSolverBody* body1, b3GpuSolverBody* body2, b3GpuSolverConstraint* c) +void resolveSingleConstraintRowGeneric2(b3GpuSolverBody* body1, b3GpuSolverBody* body2, b3GpuSolverConstraint* c) { - float deltaImpulse = c->m_rhs-b3Scalar(c->m_appliedImpulse)*c->m_cfm; - float deltaVel1Dotn = b3Dot(c->m_contactNormal,body1->m_deltaLinearVelocity) + b3Dot(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity); - float deltaVel2Dotn = -b3Dot(c->m_contactNormal,body2->m_deltaLinearVelocity) + b3Dot(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity); + float deltaImpulse = c->m_rhs - b3Scalar(c->m_appliedImpulse) * c->m_cfm; + float deltaVel1Dotn = b3Dot(c->m_contactNormal, body1->m_deltaLinearVelocity) + b3Dot(c->m_relpos1CrossNormal, body1->m_deltaAngularVelocity); + float deltaVel2Dotn = -b3Dot(c->m_contactNormal, body2->m_deltaLinearVelocity) + b3Dot(c->m_relpos2CrossNormal, body2->m_deltaAngularVelocity); - deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv; + deltaImpulse -= deltaVel1Dotn * c->m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c->m_jacDiagABInv; float sum = b3Scalar(c->m_appliedImpulse) + deltaImpulse; if (sum < c->m_lowerLimit) { - deltaImpulse = c->m_lowerLimit-b3Scalar(c->m_appliedImpulse); + deltaImpulse = c->m_lowerLimit - b3Scalar(c->m_appliedImpulse); c->m_appliedImpulse = c->m_lowerLimit; } - else if (sum > c->m_upperLimit) + else if (sum > c->m_upperLimit) { - deltaImpulse = c->m_upperLimit-b3Scalar(c->m_appliedImpulse); + deltaImpulse = c->m_upperLimit - b3Scalar(c->m_appliedImpulse); c->m_appliedImpulse = c->m_upperLimit; } else @@ -648,64 +597,56 @@ void resolveSingleConstraintRowGeneric2( b3GpuSolverBody* body1, b3GpuSolverBod c->m_appliedImpulse = sum; } - internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse); - internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse); - + internalApplyImpulse(body1, c->m_contactNormal * body1->m_invMass, c->m_angularComponentA, deltaImpulse); + internalApplyImpulse(body2, -c->m_contactNormal * body2->m_invMass, c->m_angularComponentB, deltaImpulse); } - - -void b3GpuPgsConstraintSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb) +void b3GpuPgsConstraintSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb) { - - solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); - solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); - solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + solverBody->m_deltaLinearVelocity.setValue(0.f, 0.f, 0.f); + solverBody->m_deltaAngularVelocity.setValue(0.f, 0.f, 0.f); + solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); b3Assert(rb); -// solverBody->m_worldTransform = getWorldTransform(rb); - solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass,rb->m_invMass,rb->m_invMass)); + // solverBody->m_worldTransform = getWorldTransform(rb); + solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass, rb->m_invMass, rb->m_invMass)); solverBody->m_originalBodyIndex = bodyIndex; - solverBody->m_angularFactor = b3MakeVector3(1,1,1); - solverBody->m_linearFactor = b3MakeVector3(1,1,1); + solverBody->m_angularFactor = b3MakeVector3(1, 1, 1); + solverBody->m_linearFactor = b3MakeVector3(1, 1, 1); solverBody->m_linearVelocity = getLinearVelocity(rb); solverBody->m_angularVelocity = getAngularVelocity(rb); } - -void b3GpuPgsConstraintSolver::averageVelocities() +void b3GpuPgsConstraintSolver::averageVelocities() { } - -b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints1,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints1, int numConstraints, const b3ContactSolverInfo& infoGlobal) { //only create the batches once. //@todo: incrementally update batches when constraints are added/activated and/or removed/deactivated B3_PROFILE("GpuSolveGroupCacheFriendlyIterations"); - bool createBatches = m_gpuData->m_batchSizes.size()==0; + bool createBatches = m_gpuData->m_batchSizes.size() == 0; { - if (createBatches) { - m_gpuData->m_batchSizes.resize(0); { m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); B3_PROFILE("batch joints"); - b3Assert(batchConstraints.size()==numConstraints); - int simdWidth =numConstraints+1; + b3Assert(batchConstraints.size() == numConstraints); + int simdWidth = numConstraints + 1; int numBodies = m_tmpSolverBodyPool.size(); - sortConstraintByBatch3( &batchConstraints[0], numConstraints, simdWidth , m_staticIdx, numBodies); + sortConstraintByBatch3(&batchConstraints[0], numConstraints, simdWidth, m_staticIdx, numBodies); m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); - } - } else + } + else { /*b3AlignedObjectArray<b3BatchConstraint> cpuCheckBatches; m_gpuData->m_gpuBatchConstraints->copyToHost(cpuCheckBatches); @@ -715,12 +656,11 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArr //>copyFromHost(batchConstraints); } int maxIterations = infoGlobal.m_numIterations; - + bool useBatching = true; - if (useBatching ) + if (useBatching) { - if (!useGpuSolveJointConstraintRows) { B3_PROFILE("copy to host"); @@ -730,24 +670,21 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArr m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1); m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets); gpuConstraints1->copyToHost(m_gpuData->m_cpuConstraints); - } - for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + for (int iteration = 0; iteration < maxIterations; iteration++) { - int batchOffset = 0; - int constraintOffset=0; + int constraintOffset = 0; int numBatches = m_gpuData->m_batchSizes.size(); - for (int bb=0;bb<numBatches;bb++) + for (int bb = 0; bb < numBatches; bb++) { int numConstraintsInBatch = m_gpuData->m_batchSizes[bb]; - if (useGpuSolveJointConstraintRows) { B3_PROFILE("solveJointConstraintRowsKernels"); - + /* __kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies, __global b3BatchConstraint* batchConstraints, @@ -758,53 +695,48 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArr int batchOffset, int numConstraintsInBatch*/ - - b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_solveJointConstraintRowsKernels,"m_solveJointConstraintRowsKernels"); + b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_solveJointConstraintRowsKernels, "m_solveJointConstraintRowsKernels"); launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); - launcher.setBuffer(gpuConstraints1->getBufferCL());//to detect disabled constraints + launcher.setBuffer(gpuConstraints1->getBufferCL()); //to detect disabled constraints launcher.setConst(batchOffset); launcher.setConst(numConstraintsInBatch); launcher.launch1D(numConstraintsInBatch); - - - } else//useGpu + } + else //useGpu { - - - - for (int b=0;b<numConstraintsInBatch;b++) + for (int b = 0; b < numConstraintsInBatch; b++) { - const b3BatchConstraint& c = batchConstraints[batchOffset+b]; + const b3BatchConstraint& c = batchConstraints[batchOffset + b]; /*printf("-----------\n"); printf("bb=%d\n",bb); printf("c.batchId = %d\n", c.m_batchId); */ - b3Assert(c.m_batchId==bb); + b3Assert(c.m_batchId == bb); b3GpuGenericConstraint* constraint = &m_gpuData->m_cpuConstraints[c.m_originalConstraintIndex]; - if (constraint->m_flags&B3_CONSTRAINT_FLAG_ENABLED) + if (constraint->m_flags & B3_CONSTRAINT_FLAG_ENABLED) { int numConstraintRows = m_gpuData->m_cpuConstraintInfo1[c.m_originalConstraintIndex]; int constraintOffset = m_gpuData->m_cpuConstraintRowOffsets[c.m_originalConstraintIndex]; - - for (int jj=0;jj<numConstraintRows;jj++) + + for (int jj = 0; jj < numConstraintRows; jj++) { - // - b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[constraintOffset+jj]; + // + b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[constraintOffset + jj]; //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); - resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA],&m_tmpSolverBodyPool[constraint.m_solverBodyIdB],&constraint); + resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA], &m_tmpSolverBodyPool[constraint.m_solverBodyIdB], &constraint); } } } - }//useGpu - batchOffset+=numConstraintsInBatch; - constraintOffset+=numConstraintsInBatch; + } //useGpu + batchOffset += numConstraintsInBatch; + constraintOffset += numConstraintsInBatch; } - }//for (int iteration... + } //for (int iteration... if (!useGpuSolveJointConstraintRows) { @@ -820,20 +752,16 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArr } //int sz = sizeof(b3GpuSolverBody); //printf("cpu sizeof(b3GpuSolverBody)=%d\n",sz); - - - - - - } else + } + else { - for ( int iteration = 0 ; iteration< maxIterations ; iteration++) - { - int numJoints = m_tmpSolverNonContactConstraintPool.size(); - for (int j=0;j<numJoints;j++) + for (int iteration = 0; iteration < maxIterations; iteration++) + { + int numJoints = m_tmpSolverNonContactConstraintPool.size(); + for (int j = 0; j < numJoints; j++) { b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA],&m_tmpSolverBodyPool[constraint.m_solverBodyIdB],&constraint); + resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA], &m_tmpSolverBodyPool[constraint.m_solverBodyIdB], &constraint); } if (!m_usePgs) @@ -842,212 +770,198 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArr } } } - } clFinish(m_gpuData->m_queue); return 0.f; } - - - static b3AlignedObjectArray<int> bodyUsed; static b3AlignedObjectArray<int> curUsed; - - -inline int b3GpuPgsConstraintSolver::sortConstraintByBatch3( b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies) +inline int b3GpuPgsConstraintSolver::sortConstraintByBatch3(b3BatchConstraint* cs, int numConstraints, int simdWidth, int staticIdx, int numBodies) { //int sz = sizeof(b3BatchConstraint); B3_PROFILE("sortConstraintByBatch3"); - + static int maxSwaps = 0; int numSwaps = 0; - curUsed.resize(2*simdWidth); + curUsed.resize(2 * simdWidth); static int maxNumConstraints = 0; - if (maxNumConstraints<numConstraints) + if (maxNumConstraints < numConstraints) { maxNumConstraints = numConstraints; //printf("maxNumConstraints = %d\n",maxNumConstraints ); } - int numUsedArray = numBodies/32+1; + int numUsedArray = numBodies / 32 + 1; bodyUsed.resize(numUsedArray); - for (int q=0;q<numUsedArray;q++) - bodyUsed[q]=0; + for (int q = 0; q < numUsedArray; q++) + bodyUsed[q] = 0; - int curBodyUsed = 0; int numIter = 0; - - + #if defined(_DEBUG) - for(int i=0; i<numConstraints; i++) + for (int i = 0; i < numConstraints; i++) cs[i].m_batchId = -1; #endif - + int numValidConstraints = 0; -// int unprocessedConstraintIndex = 0; + // int unprocessedConstraintIndex = 0; int batchIdx = 0; - { B3_PROFILE("cpu batch innerloop"); - - while( numValidConstraints < numConstraints) + + while (numValidConstraints < numConstraints) { numIter++; int nCurrentBatch = 0; // clear flag - for(int i=0; i<curBodyUsed; i++) - bodyUsed[curUsed[i]/32] = 0; + for (int i = 0; i < curBodyUsed; i++) + bodyUsed[curUsed[i] / 32] = 0; - curBodyUsed = 0; + curBodyUsed = 0; - for(int i=numValidConstraints; i<numConstraints; i++) + for (int i = numValidConstraints; i < numConstraints; i++) { int idx = i; - b3Assert( idx < numConstraints ); + b3Assert(idx < numConstraints); // check if it can go int bodyAS = cs[idx].m_bodyAPtrAndSignBit; int bodyBS = cs[idx].m_bodyBPtrAndSignBit; int bodyA = abs(bodyAS); int bodyB = abs(bodyBS); - bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx; - bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx; + bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; + bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; int aUnavailable = 0; int bUnavailable = 0; if (!aIsStatic) { - aUnavailable = bodyUsed[ bodyA/32 ] & (1<<(bodyA&31)); + aUnavailable = bodyUsed[bodyA / 32] & (1 << (bodyA & 31)); } if (!aUnavailable) - if (!bIsStatic) - { - bUnavailable = bodyUsed[ bodyB/32 ] & (1<<(bodyB&31)); - } - - if( aUnavailable==0 && bUnavailable==0 ) // ok + if (!bIsStatic) + { + bUnavailable = bodyUsed[bodyB / 32] & (1 << (bodyB & 31)); + } + + if (aUnavailable == 0 && bUnavailable == 0) // ok { if (!aIsStatic) { - bodyUsed[ bodyA/32 ] |= (1<<(bodyA&31)); - curUsed[curBodyUsed++]=bodyA; + bodyUsed[bodyA / 32] |= (1 << (bodyA & 31)); + curUsed[curBodyUsed++] = bodyA; } if (!bIsStatic) { - bodyUsed[ bodyB/32 ] |= (1<<(bodyB&31)); - curUsed[curBodyUsed++]=bodyB; + bodyUsed[bodyB / 32] |= (1 << (bodyB & 31)); + curUsed[curBodyUsed++] = bodyB; } cs[idx].m_batchId = batchIdx; - if (i!=numValidConstraints) + if (i != numValidConstraints) { - b3Swap(cs[i],cs[numValidConstraints]); + b3Swap(cs[i], cs[numValidConstraints]); numSwaps++; } numValidConstraints++; { nCurrentBatch++; - if( nCurrentBatch == simdWidth ) + if (nCurrentBatch == simdWidth) { nCurrentBatch = 0; - for(int i=0; i<curBodyUsed; i++) - bodyUsed[curUsed[i]/32] = 0; + for (int i = 0; i < curBodyUsed; i++) + bodyUsed[curUsed[i] / 32] = 0; curBodyUsed = 0; } } } } m_gpuData->m_batchSizes.push_back(nCurrentBatch); - batchIdx ++; + batchIdx++; } } - + #if defined(_DEBUG) - // debugPrintf( "nBatches: %d\n", batchIdx ); - for(int i=0; i<numConstraints; i++) - { - b3Assert( cs[i].m_batchId != -1 ); - } + // debugPrintf( "nBatches: %d\n", batchIdx ); + for (int i = 0; i < numConstraints; i++) + { + b3Assert(cs[i].m_batchId != -1); + } #endif - if (maxSwaps<numSwaps) + if (maxSwaps < numSwaps) { maxSwaps = numSwaps; //printf("maxSwaps = %d\n", maxSwaps); } - + return batchIdx; } - /// b3PgsJacobiSolver Sequentially applies impulses -b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, - int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints, const b3ContactSolverInfo& infoGlobal) +b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, + int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) { - B3_PROFILE("solveJoints"); //you need to provide at least some bodies - - solveGroupCacheFriendlySetup( gpuBodies, gpuInertias,numBodies,gpuConstraints, numConstraints,infoGlobal); - solveGroupCacheFriendlyIterations(gpuConstraints, numConstraints,infoGlobal); + solveGroupCacheFriendlySetup(gpuBodies, gpuInertias, numBodies, gpuConstraints, numConstraints, infoGlobal); + + solveGroupCacheFriendlyIterations(gpuConstraints, numConstraints, infoGlobal); + + solveGroupCacheFriendlyFinish(gpuBodies, gpuInertias, numBodies, gpuConstraints, numConstraints, infoGlobal); - solveGroupCacheFriendlyFinish(gpuBodies, gpuInertias,numBodies, gpuConstraints, numConstraints, infoGlobal); - return 0.f; } -void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, - int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints) +void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, + int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints) { b3ContactSolverInfo infoGlobal; infoGlobal.m_splitImpulse = false; - infoGlobal.m_timeStep = 1.f/60.f; - infoGlobal.m_numIterations = 4;//4; -// infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION; + infoGlobal.m_timeStep = 1.f / 60.f; + infoGlobal.m_numIterations = 4; //4; + // infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION; //infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS; - infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS; + infoGlobal.m_solverMode |= B3_SOLVER_USE_2_FRICTION_DIRECTIONS; //if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) //if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) - - - solveGroup(gpuBodies,gpuInertias,numBodies,gpuConstraints,numConstraints,infoGlobal); + solveGroup(gpuBodies, gpuInertias, numBodies, gpuConstraints, numConstraints, infoGlobal); } //b3AlignedObjectArray<b3RigidBodyData> testBodies; - -b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyData>* gpuBodies,b3OpenCLArray<b3InertiaData>* gpuInertias,int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal) +b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal) { B3_PROFILE("solveGroupCacheFriendlyFinish"); -// int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); -// int i,j; - + // int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + // int i,j; { if (gpuBreakConstraints) { B3_PROFILE("breakViolatedConstraintsKernel"); - b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_breakViolatedConstraintsKernel,"m_breakViolatedConstraintsKernel"); + b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_breakViolatedConstraintsKernel, "m_breakViolatedConstraintsKernel"); launcher.setBuffer(gpuConstraints->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL()); launcher.setConst(numConstraints); launcher.launch1D(numConstraints); - } else + } + else { gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); m_gpuData->m_gpuBatchConstraints->copyToHost(m_gpuData->m_cpuBatchConstraints); @@ -1056,31 +970,28 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1); m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets); - for (int cid=0;cid<numConstraints;cid++) + for (int cid = 0; cid < numConstraints; cid++) { int originalConstraintIndex = batchConstraints[cid].m_originalConstraintIndex; int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[originalConstraintIndex]; int numRows = m_gpuData->m_cpuConstraintInfo1[originalConstraintIndex]; if (numRows) { - - // printf("cid=%d, breakingThreshold =%f\n",cid,breakingThreshold); - for (int i=0;i<numRows;i++) + // printf("cid=%d, breakingThreshold =%f\n",cid,breakingThreshold); + for (int i = 0; i < numRows; i++) { - int rowIndex =constraintRowOffset+i; + int rowIndex = constraintRowOffset + i; int orgConstraintIndex = m_gpuData->m_cpuConstraintRows[rowIndex].m_originalConstraintIndex; float breakingThreshold = m_gpuData->m_cpuConstraints[orgConstraintIndex].m_breakingImpulseThreshold; - // printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse); + // printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse); if (b3Fabs(m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold) { - - m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED; + m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags = 0; //&= ~B3_CONSTRAINT_FLAG_ENABLED; } } } } - gpuConstraints->copyFromHost(m_gpuData->m_cpuConstraints); } } @@ -1090,28 +1001,27 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b { B3_PROFILE("GPU write back velocities and transforms"); - b3LauncherCL launcher(m_gpuData->m_queue,m_gpuData->m_writeBackVelocitiesKernel,"m_writeBackVelocitiesKernel"); + b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_writeBackVelocitiesKernel, "m_writeBackVelocitiesKernel"); launcher.setBuffer(gpuBodies->getBufferCL()); launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); launcher.setConst(numBodies); launcher.launch1D(numBodies); clFinish(m_gpuData->m_queue); -// m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); -// m_gpuData->m_gpuBodies->copyToHostPointer(bodies,numBodies); + // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + // m_gpuData->m_gpuBodies->copyToHostPointer(bodies,numBodies); //m_gpuData->m_gpuBodies->copyToHost(testBodies); - - } + } else { B3_PROFILE("CPU write back velocities and transforms"); m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); gpuBodies->copyToHost(m_gpuData->m_cpuBodies); - for ( int i=0;i<m_tmpSolverBodyPool.size();i++) + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) { int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; //printf("bodyIndex=%d\n",bodyIndex); - b3Assert(i==bodyIndex); + b3Assert(i == bodyIndex); b3RigidBodyData* body = &m_gpuData->m_cpuBodies[bodyIndex]; if (body->m_invMass) @@ -1125,11 +1035,12 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b { body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity; body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity; - } else + } + else { b3Assert(0); } - /* + /* if (infoGlobal.m_splitImpulse) { body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin(); @@ -1139,10 +1050,9 @@ b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b } */ } - }//for + } //for gpuBodies->copyFromHost(m_gpuData->m_cpuBodies); - } } diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h index ec0e3f73d6..00bc544f02 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h @@ -19,7 +19,6 @@ subject to the following restrictions: struct b3Contact4; struct b3ContactPoint; - class b3Dispatcher; #include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" @@ -38,41 +37,40 @@ class b3GpuPgsConstraintSolver protected: int m_staticIdx; struct b3GpuPgsJacobiSolverInternalData* m_gpuData; - protected: - b3AlignedObjectArray<b3GpuSolverBody> m_tmpSolverBodyPool; - b3GpuConstraintArray m_tmpSolverContactConstraintPool; - b3GpuConstraintArray m_tmpSolverNonContactConstraintPool; - b3GpuConstraintArray m_tmpSolverContactFrictionConstraintPool; - b3GpuConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + +protected: + b3AlignedObjectArray<b3GpuSolverBody> m_tmpSolverBodyPool; + b3GpuConstraintArray m_tmpSolverContactConstraintPool; + b3GpuConstraintArray m_tmpSolverNonContactConstraintPool; + b3GpuConstraintArray m_tmpSolverContactFrictionConstraintPool; + b3GpuConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; b3AlignedObjectArray<unsigned int> m_tmpConstraintSizesPool; - - bool m_usePgs; - void averageVelocities(); + bool m_usePgs; + void averageVelocities(); - int m_maxOverrideNumSolverIterations; + int m_maxOverrideNumSolverIterations; - int m_numSplitImpulseRecoveries; + int m_numSplitImpulseRecoveries; -// int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias); - void initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb); + // int getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies,b3InertiaData* inertias); + void initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb); public: - b3GpuPgsConstraintSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs); - virtual~b3GpuPgsConstraintSolver (); - - virtual b3Scalar solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints1,int numConstraints,const b3ContactSolverInfo& infoGlobal); - virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyData>* gpuBodies,b3OpenCLArray<b3InertiaData>* gpuInertias,int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); + b3GpuPgsConstraintSolver(cl_context ctx, cl_device_id device, cl_command_queue queue, bool usePgs); + virtual ~b3GpuPgsConstraintSolver(); + virtual b3Scalar solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints1, int numConstraints, const b3ContactSolverInfo& infoGlobal); + virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); + b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); - b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies,b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal); - void solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, - int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints); + b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal); + void solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, + int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints); - int sortConstraintByBatch3( struct b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies); - void recomputeBatches(); + int sortConstraintByBatch3(struct b3BatchConstraint* cs, int numConstraints, int simdWidth, int staticIdx, int numBodies); + void recomputeBatches(); }; -#endif //B3_GPU_PGS_CONSTRAINT_SOLVER_H +#endif //B3_GPU_PGS_CONSTRAINT_SOLVER_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp index f0b0abd5e0..e3d235a4fd 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp @@ -2,7 +2,7 @@ bool gUseLargeBatches = false; bool gCpuBatchContacts = false; bool gCpuSolveConstraint = false; -bool gCpuRadixSort=false; +bool gCpuRadixSort = false; bool gCpuSetSortData = false; bool gCpuSortContactsDeterminism = false; bool gUseCpuCopyConstraints = false; @@ -11,7 +11,6 @@ bool gReorderContactsOnCpu = false; bool optionalSortContactsDeterminism = true; - #include "b3GpuPgsContactSolver.h" #include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" @@ -23,7 +22,6 @@ bool optionalSortContactsDeterminism = true; #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" #include "b3Solver.h" - #define B3_SOLVER_SETUP_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl" #define B3_SOLVER_SETUP2_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl" #define B3_SOLVER_CONTACT_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl" @@ -38,11 +36,7 @@ bool optionalSortContactsDeterminism = true; #include "kernels/batchingKernels.h" #include "kernels/batchingKernelsNew.h" - - - - -struct b3GpuBatchingPgsSolverInternalData +struct b3GpuBatchingPgsSolverInternalData { cl_context m_context; cl_device_id m_device; @@ -53,9 +47,9 @@ struct b3GpuBatchingPgsSolverInternalData b3OpenCLArray<b3GpuConstraint4>* m_contactCGPU; b3OpenCLArray<unsigned int>* m_numConstraints; b3OpenCLArray<unsigned int>* m_offsets; - - b3Solver* m_solverGPU; - + + b3Solver* m_solverGPU; + cl_kernel m_batchingKernel; cl_kernel m_batchingKernelNew; cl_kernel m_solveContactKernel; @@ -67,17 +61,14 @@ struct b3GpuBatchingPgsSolverInternalData cl_kernel m_reorderContactKernel; cl_kernel m_copyConstraintKernel; - cl_kernel m_setDeterminismSortDataBodyAKernel; - cl_kernel m_setDeterminismSortDataBodyBKernel; - cl_kernel m_setDeterminismSortDataChildShapeAKernel; - cl_kernel m_setDeterminismSortDataChildShapeBKernel; - + cl_kernel m_setDeterminismSortDataBodyAKernel; + cl_kernel m_setDeterminismSortDataBodyBKernel; + cl_kernel m_setDeterminismSortDataChildShapeAKernel; + cl_kernel m_setDeterminismSortDataChildShapeBKernel; - - - class b3RadixSort32CL* m_sort32; - class b3BoundSearchCL* m_search; - class b3PrefixScanCL* m_scan; + class b3RadixSort32CL* m_sort32; + class b3BoundSearchCL* m_search; + class b3PrefixScanCL* m_scan; b3OpenCLArray<b3SortData>* m_sortDataBuffer; b3OpenCLArray<b3Contact4>* m_contactBuffer; @@ -85,63 +76,56 @@ struct b3GpuBatchingPgsSolverInternalData b3OpenCLArray<b3RigidBodyData>* m_bodyBufferGPU; b3OpenCLArray<b3InertiaData>* m_inertiaBufferGPU; b3OpenCLArray<b3Contact4>* m_pBufContactOutGPU; - - b3OpenCLArray<b3Contact4>* m_pBufContactOutGPUCopy; - b3OpenCLArray<b3SortData>* m_contactKeyValues; + b3OpenCLArray<b3Contact4>* m_pBufContactOutGPUCopy; + b3OpenCLArray<b3SortData>* m_contactKeyValues; b3AlignedObjectArray<unsigned int> m_idxBuffer; b3AlignedObjectArray<b3SortData> m_sortData; b3AlignedObjectArray<b3Contact4> m_old; - b3AlignedObjectArray<int> m_batchSizes; - b3OpenCLArray<int>* m_batchSizesGpu; - + b3AlignedObjectArray<int> m_batchSizes; + b3OpenCLArray<int>* m_batchSizesGpu; }; - - -b3GpuPgsContactSolver::b3GpuPgsContactSolver(cl_context ctx,cl_device_id device, cl_command_queue q,int pairCapacity) +b3GpuPgsContactSolver::b3GpuPgsContactSolver(cl_context ctx, cl_device_id device, cl_command_queue q, int pairCapacity) { - m_debugOutput=0; + m_debugOutput = 0; m_data = new b3GpuBatchingPgsSolverInternalData; m_data->m_context = ctx; m_data->m_device = device; m_data->m_queue = q; m_data->m_pairCapacity = pairCapacity; m_data->m_nIterations = 4; - m_data->m_batchSizesGpu = new b3OpenCLArray<int>(ctx,q); - m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx,q); - m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx,q); - m_data->m_pBufContactOutGPU = new b3OpenCLArray<b3Contact4>(ctx,q); - - m_data->m_pBufContactOutGPUCopy = new b3OpenCLArray<b3Contact4>(ctx,q); - m_data->m_contactKeyValues = new b3OpenCLArray<b3SortData>(ctx,q); + m_data->m_batchSizesGpu = new b3OpenCLArray<int>(ctx, q); + m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyData>(ctx, q); + m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaData>(ctx, q); + m_data->m_pBufContactOutGPU = new b3OpenCLArray<b3Contact4>(ctx, q); + m_data->m_pBufContactOutGPUCopy = new b3OpenCLArray<b3Contact4>(ctx, q); + m_data->m_contactKeyValues = new b3OpenCLArray<b3SortData>(ctx, q); - m_data->m_solverGPU = new b3Solver(ctx,device,q,512*1024); + m_data->m_solverGPU = new b3Solver(ctx, device, q, 512 * 1024); - m_data->m_sort32 = new b3RadixSort32CL(ctx,device,m_data->m_queue); - m_data->m_scan = new b3PrefixScanCL(ctx,device,m_data->m_queue,B3_SOLVER_N_CELLS); - m_data->m_search = new b3BoundSearchCL(ctx,device,m_data->m_queue,B3_SOLVER_N_CELLS); + m_data->m_sort32 = new b3RadixSort32CL(ctx, device, m_data->m_queue); + m_data->m_scan = new b3PrefixScanCL(ctx, device, m_data->m_queue, B3_SOLVER_N_CELLS); + m_data->m_search = new b3BoundSearchCL(ctx, device, m_data->m_queue, B3_SOLVER_N_CELLS); - const int sortSize = B3NEXTMULTIPLEOF( pairCapacity, 512 ); + const int sortSize = B3NEXTMULTIPLEOF(pairCapacity, 512); - m_data->m_sortDataBuffer = new b3OpenCLArray<b3SortData>(ctx,m_data->m_queue,sortSize); - m_data->m_contactBuffer = new b3OpenCLArray<b3Contact4>(ctx,m_data->m_queue); + m_data->m_sortDataBuffer = new b3OpenCLArray<b3SortData>(ctx, m_data->m_queue, sortSize); + m_data->m_contactBuffer = new b3OpenCLArray<b3Contact4>(ctx, m_data->m_queue); - m_data->m_numConstraints = new b3OpenCLArray<unsigned int>(ctx,m_data->m_queue,B3_SOLVER_N_CELLS); + m_data->m_numConstraints = new b3OpenCLArray<unsigned int>(ctx, m_data->m_queue, B3_SOLVER_N_CELLS); m_data->m_numConstraints->resize(B3_SOLVER_N_CELLS); - m_data->m_contactCGPU = new b3OpenCLArray<b3GpuConstraint4>(ctx,q,pairCapacity); + m_data->m_contactCGPU = new b3OpenCLArray<b3GpuConstraint4>(ctx, q, pairCapacity); - m_data->m_offsets = new b3OpenCLArray<unsigned int>( ctx,m_data->m_queue,B3_SOLVER_N_CELLS); + m_data->m_offsets = new b3OpenCLArray<unsigned int>(ctx, m_data->m_queue, B3_SOLVER_N_CELLS); m_data->m_offsets->resize(B3_SOLVER_N_CELLS); const char* additionalMacros = ""; //const char* srcFileNameForCaching=""; - - cl_int pErrNum; const char* batchKernelSource = batchingKernelsCL; const char* batchKernelNewSource = batchingKernelsNewCL; @@ -149,88 +133,73 @@ b3GpuPgsContactSolver::b3GpuPgsContactSolver(cl_context ctx,cl_device_id device, const char* solverSetup2Source = solverSetup2CL; const char* solveContactSource = solveContactCL; const char* solveFrictionSource = solveFrictionCL; - - + { - - cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveContactSource, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH); + cl_program solveContactProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solveContactSource, &pErrNum, additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH); b3Assert(solveContactProg); - - cl_program solveFrictionProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveFrictionSource, &pErrNum,additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH); + + cl_program solveFrictionProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solveFrictionSource, &pErrNum, additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH); b3Assert(solveFrictionProg); - cl_program solverSetup2Prog= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetup2Source, &pErrNum,additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH); - - + cl_program solverSetup2Prog = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solverSetup2Source, &pErrNum, additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH); + b3Assert(solverSetup2Prog); - - cl_program solverSetupProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetupSource, &pErrNum,additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH); + cl_program solverSetupProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solverSetupSource, &pErrNum, additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH); b3Assert(solverSetupProg); - - - m_data->m_solveFrictionKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg,additionalMacros ); + + m_data->m_solveFrictionKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg, additionalMacros); b3Assert(m_data->m_solveFrictionKernel); - m_data->m_solveContactKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg,additionalMacros ); + m_data->m_solveContactKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg, additionalMacros); b3Assert(m_data->m_solveContactKernel); - m_data->m_solveSingleContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "solveSingleContactKernel", &pErrNum, solveContactProg,additionalMacros ); + m_data->m_solveSingleContactKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solveContactSource, "solveSingleContactKernel", &pErrNum, solveContactProg, additionalMacros); b3Assert(m_data->m_solveSingleContactKernel); - m_data->m_solveSingleFrictionKernel =b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "solveSingleFrictionKernel", &pErrNum, solveFrictionProg,additionalMacros ); + m_data->m_solveSingleFrictionKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solveFrictionSource, "solveSingleFrictionKernel", &pErrNum, solveFrictionProg, additionalMacros); b3Assert(m_data->m_solveSingleFrictionKernel); - - m_data->m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg,additionalMacros ); + + m_data->m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg, additionalMacros); b3Assert(m_data->m_contactToConstraintKernel); - - m_data->m_setSortDataKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + + m_data->m_setSortDataKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_data->m_setSortDataKernel); - m_data->m_setDeterminismSortDataBodyAKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetDeterminismSortDataBodyA", &pErrNum, solverSetup2Prog,additionalMacros ); + m_data->m_setDeterminismSortDataBodyAKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "SetDeterminismSortDataBodyA", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_data->m_setDeterminismSortDataBodyAKernel); - m_data->m_setDeterminismSortDataBodyBKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetDeterminismSortDataBodyB", &pErrNum, solverSetup2Prog,additionalMacros ); + m_data->m_setDeterminismSortDataBodyBKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "SetDeterminismSortDataBodyB", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_data->m_setDeterminismSortDataBodyBKernel); - m_data->m_setDeterminismSortDataChildShapeAKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetDeterminismSortDataChildShapeA", &pErrNum, solverSetup2Prog,additionalMacros ); + m_data->m_setDeterminismSortDataChildShapeAKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "SetDeterminismSortDataChildShapeA", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_data->m_setDeterminismSortDataChildShapeAKernel); - m_data->m_setDeterminismSortDataChildShapeBKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetDeterminismSortDataChildShapeB", &pErrNum, solverSetup2Prog,additionalMacros ); + m_data->m_setDeterminismSortDataChildShapeBKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "SetDeterminismSortDataChildShapeB", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_data->m_setDeterminismSortDataChildShapeBKernel); - - m_data->m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + m_data->m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_data->m_reorderContactKernel); - - m_data->m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + m_data->m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_data->m_copyConstraintKernel); - } { - cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelSource, &pErrNum,additionalMacros, B3_BATCHING_PATH); + cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, batchKernelSource, &pErrNum, additionalMacros, B3_BATCHING_PATH); b3Assert(batchingProg); - - m_data->m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg,additionalMacros ); + + m_data->m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg, additionalMacros); b3Assert(m_data->m_batchingKernel); } - + { - cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelNewSource, &pErrNum,additionalMacros, B3_BATCHING_NEW_PATH); + cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, batchKernelNewSource, &pErrNum, additionalMacros, B3_BATCHING_NEW_PATH); b3Assert(batchingNewProg); - - m_data->m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg,additionalMacros ); + + m_data->m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString(ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg, additionalMacros); b3Assert(m_data->m_batchingKernelNew); } - - - - - - - } b3GpuPgsContactSolver::~b3GpuPgsContactSolver() @@ -242,8 +211,6 @@ b3GpuPgsContactSolver::~b3GpuPgsContactSolver() delete m_data->m_pBufContactOutGPUCopy; delete m_data->m_contactKeyValues; - - delete m_data->m_contactCGPU; delete m_data->m_numConstraints; delete m_data->m_offsets; @@ -259,29 +226,25 @@ b3GpuPgsContactSolver::~b3GpuPgsContactSolver() clReleaseKernel(m_data->m_batchingKernelNew); clReleaseKernel(m_data->m_solveSingleContactKernel); clReleaseKernel(m_data->m_solveSingleFrictionKernel); - clReleaseKernel( m_data->m_solveContactKernel); - clReleaseKernel( m_data->m_solveFrictionKernel); + clReleaseKernel(m_data->m_solveContactKernel); + clReleaseKernel(m_data->m_solveFrictionKernel); - clReleaseKernel( m_data->m_contactToConstraintKernel); - clReleaseKernel( m_data->m_setSortDataKernel); - clReleaseKernel( m_data->m_reorderContactKernel); - clReleaseKernel( m_data->m_copyConstraintKernel); + clReleaseKernel(m_data->m_contactToConstraintKernel); + clReleaseKernel(m_data->m_setSortDataKernel); + clReleaseKernel(m_data->m_reorderContactKernel); + clReleaseKernel(m_data->m_copyConstraintKernel); clReleaseKernel(m_data->m_setDeterminismSortDataBodyAKernel); clReleaseKernel(m_data->m_setDeterminismSortDataBodyBKernel); clReleaseKernel(m_data->m_setDeterminismSortDataChildShapeAKernel); clReleaseKernel(m_data->m_setDeterminismSortDataChildShapeBKernel); - - delete m_data; } - - struct b3ConstraintCfg { - b3ConstraintCfg( float dt = 0.f ): m_positionDrift( 0.005f ), m_positionConstraintCoeff( 0.2f ), m_dt(dt), m_staticIdx(0) {} + b3ConstraintCfg(float dt = 0.f) : m_positionDrift(0.005f), m_positionConstraintCoeff(0.2f), m_dt(dt), m_staticIdx(0) {} float m_positionDrift; float m_positionConstraintCoeff; @@ -291,354 +254,306 @@ struct b3ConstraintCfg int m_staticIdx; }; - - -void b3GpuPgsContactSolver::solveContactConstraintBatchSizes( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches,int numIterations, const b3AlignedObjectArray<int>* batchSizes)//const b3OpenCLArray<int>* gpuBatchSizes) +void b3GpuPgsContactSolver::solveContactConstraintBatchSizes(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches, int numIterations, const b3AlignedObjectArray<int>* batchSizes) //const b3OpenCLArray<int>* gpuBatchSizes) { B3_PROFILE("solveContactConstraintBatchSizes"); - int numBatches = batchSizes->size()/B3_MAX_NUM_BATCHES; - for(int iter=0; iter<numIterations; iter++) + int numBatches = batchSizes->size() / B3_MAX_NUM_BATCHES; + for (int iter = 0; iter < numIterations; iter++) { - - for (int cellId=0;cellId<numBatches;cellId++) + for (int cellId = 0; cellId < numBatches; cellId++) { int offset = 0; - for (int ii=0;ii<B3_MAX_NUM_BATCHES;ii++) + for (int ii = 0; ii < B3_MAX_NUM_BATCHES; ii++) { - int numInBatch = batchSizes->at(cellId*B3_MAX_NUM_BATCHES+ii); + int numInBatch = batchSizes->at(cellId * B3_MAX_NUM_BATCHES + ii); if (!numInBatch) break; { - b3LauncherCL launcher( m_data->m_queue, m_data->m_solveSingleContactKernel,"m_solveSingleContactKernel" ); - launcher.setBuffer(bodyBuf->getBufferCL() ); - launcher.setBuffer(shapeBuf->getBufferCL() ); - launcher.setBuffer( constraint->getBufferCL() ); + b3LauncherCL launcher(m_data->m_queue, m_data->m_solveSingleContactKernel, "m_solveSingleContactKernel"); + launcher.setBuffer(bodyBuf->getBufferCL()); + launcher.setBuffer(shapeBuf->getBufferCL()); + launcher.setBuffer(constraint->getBufferCL()); launcher.setConst(cellId); launcher.setConst(offset); launcher.setConst(numInBatch); launcher.launch1D(numInBatch); - offset+=numInBatch; + offset += numInBatch; } } } } - - for(int iter=0; iter<numIterations; iter++) + for (int iter = 0; iter < numIterations; iter++) { - for (int cellId=0;cellId<numBatches;cellId++) + for (int cellId = 0; cellId < numBatches; cellId++) { int offset = 0; - for (int ii=0;ii<B3_MAX_NUM_BATCHES;ii++) + for (int ii = 0; ii < B3_MAX_NUM_BATCHES; ii++) { - int numInBatch = batchSizes->at(cellId*B3_MAX_NUM_BATCHES+ii); + int numInBatch = batchSizes->at(cellId * B3_MAX_NUM_BATCHES + ii); if (!numInBatch) break; { - b3LauncherCL launcher( m_data->m_queue, m_data->m_solveSingleFrictionKernel,"m_solveSingleFrictionKernel" ); - launcher.setBuffer(bodyBuf->getBufferCL() ); - launcher.setBuffer(shapeBuf->getBufferCL() ); - launcher.setBuffer( constraint->getBufferCL() ); + b3LauncherCL launcher(m_data->m_queue, m_data->m_solveSingleFrictionKernel, "m_solveSingleFrictionKernel"); + launcher.setBuffer(bodyBuf->getBufferCL()); + launcher.setBuffer(shapeBuf->getBufferCL()); + launcher.setBuffer(constraint->getBufferCL()); launcher.setConst(cellId); launcher.setConst(offset); launcher.setConst(numInBatch); launcher.launch1D(numInBatch); - offset+=numInBatch; + offset += numInBatch; } } } } } -void b3GpuPgsContactSolver::solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches,int numIterations, const b3AlignedObjectArray<int>* batchSizes)//,const b3OpenCLArray<int>* gpuBatchSizes) +void b3GpuPgsContactSolver::solveContactConstraint(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches, int numIterations, const b3AlignedObjectArray<int>* batchSizes) //,const b3OpenCLArray<int>* gpuBatchSizes) { - //sort the contacts - - b3Int4 cdata = b3MakeInt4( n, 0, 0, 0 ); + b3Int4 cdata = b3MakeInt4(n, 0, 0, 0); { - const int nn = B3_SOLVER_N_CELLS; cdata.x = 0; - cdata.y = maxNumBatches;//250; - + cdata.y = maxNumBatches; //250; - int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; + int numWorkItems = 64 * nn / B3_SOLVER_N_BATCHES; #ifdef DEBUG_ME - SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; - adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device,numWorkItems); + SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; + adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device, numWorkItems); #endif - - { - B3_PROFILE("m_batchSolveKernel iterations"); - for(int iter=0; iter<numIterations; iter++) + for (int iter = 0; iter < numIterations; iter++) { - for(int ib=0; ib<B3_SOLVER_N_BATCHES; ib++) + for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++) { #ifdef DEBUG_ME - memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems); - gpuDebugInfo.write(debugInfo,numWorkItems); + memset(debugInfo, 0, sizeof(SolverDebugInfo) * numWorkItems); + gpuDebugInfo.write(debugInfo, numWorkItems); #endif - cdata.z = ib; - - b3LauncherCL launcher( m_data->m_queue, m_data->m_solveContactKernel,"m_solveContactKernel" ); + b3LauncherCL launcher(m_data->m_queue, m_data->m_solveContactKernel, "m_solveContactKernel"); #if 1 - - b3BufferInfoCL bInfo[] = { - - b3BufferInfoCL( bodyBuf->getBufferCL() ), - b3BufferInfoCL( shapeBuf->getBufferCL() ), - b3BufferInfoCL( constraint->getBufferCL() ), - b3BufferInfoCL( m_data->m_solverGPU->m_numConstraints->getBufferCL() ), - b3BufferInfoCL( m_data->m_solverGPU->m_offsets->getBufferCL() ) + + b3BufferInfoCL bInfo[] = { + + b3BufferInfoCL(bodyBuf->getBufferCL()), + b3BufferInfoCL(shapeBuf->getBufferCL()), + b3BufferInfoCL(constraint->getBufferCL()), + b3BufferInfoCL(m_data->m_solverGPU->m_numConstraints->getBufferCL()), + b3BufferInfoCL(m_data->m_solverGPU->m_offsets->getBufferCL()) #ifdef DEBUG_ME - , b3BufferInfoCL(&gpuDebugInfo) + , + b3BufferInfoCL(&gpuDebugInfo) #endif - }; - - + }; - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); - launcher.setBuffer( m_data->m_solverGPU->m_batchSizes.getBufferCL()); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); + launcher.setBuffer(m_data->m_solverGPU->m_batchSizes.getBufferCL()); //launcher.setConst( cdata.x ); - launcher.setConst( cdata.y ); - launcher.setConst( cdata.z ); + launcher.setConst(cdata.y); + launcher.setConst(cdata.z); b3Int4 nSplit; nSplit.x = B3_SOLVER_N_SPLIT_X; nSplit.y = B3_SOLVER_N_SPLIT_Y; nSplit.z = B3_SOLVER_N_SPLIT_Z; - launcher.setConst( nSplit ); - launcher.launch1D( numWorkItems, 64 ); + launcher.setConst(nSplit); + launcher.launch1D(numWorkItems, 64); - #else - const char* fileName = "m_batchSolveKernel.bin"; - FILE* f = fopen(fileName,"rb"); - if (f) - { - int sizeInBytes=0; - if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET)) - { - printf("error, cannot get file size\n"); - exit(0); - } - - unsigned char* buf = (unsigned char*) malloc(sizeInBytes); - fread(buf,sizeInBytes,1,f); - int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes,m_context); - int num = *(int*)&buf[serializedBytes]; - - launcher.launch1D( num); - - //this clFinish is for testing on errors - clFinish(m_queue); - } + const char* fileName = "m_batchSolveKernel.bin"; + FILE* f = fopen(fileName, "rb"); + if (f) + { + int sizeInBytes = 0; + if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET)) + { + printf("error, cannot get file size\n"); + exit(0); + } + + unsigned char* buf = (unsigned char*)malloc(sizeInBytes); + fread(buf, sizeInBytes, 1, f); + int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes, m_context); + int num = *(int*)&buf[serializedBytes]; + + launcher.launch1D(num); + + //this clFinish is for testing on errors + clFinish(m_queue); + } #endif - #ifdef DEBUG_ME clFinish(m_queue); - gpuDebugInfo.read(debugInfo,numWorkItems); + gpuDebugInfo.read(debugInfo, numWorkItems); clFinish(m_queue); - for (int i=0;i<numWorkItems;i++) + for (int i = 0; i < numWorkItems; i++) { - if (debugInfo[i].m_valInt2>0) + if (debugInfo[i].m_valInt2 > 0) { - printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2); + printf("debugInfo[i].m_valInt2 = %d\n", i, debugInfo[i].m_valInt2); } - if (debugInfo[i].m_valInt3>0) + if (debugInfo[i].m_valInt3 > 0) { - printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3); + printf("debugInfo[i].m_valInt3 = %d\n", i, debugInfo[i].m_valInt3); } } -#endif //DEBUG_ME - - +#endif //DEBUG_ME } } - - clFinish(m_data->m_queue); - + clFinish(m_data->m_queue); } cdata.x = 1; - bool applyFriction=true; + bool applyFriction = true; if (applyFriction) - { + { B3_PROFILE("m_batchSolveKernel iterations2"); - for(int iter=0; iter<numIterations; iter++) + for (int iter = 0; iter < numIterations; iter++) { - for(int ib=0; ib<B3_SOLVER_N_BATCHES; ib++) + for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++) { cdata.z = ib; - - - b3BufferInfoCL bInfo[] = { - b3BufferInfoCL( bodyBuf->getBufferCL() ), - b3BufferInfoCL( shapeBuf->getBufferCL() ), - b3BufferInfoCL( constraint->getBufferCL() ), - b3BufferInfoCL( m_data->m_solverGPU->m_numConstraints->getBufferCL() ), - b3BufferInfoCL( m_data->m_solverGPU->m_offsets->getBufferCL() ) + + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL(bodyBuf->getBufferCL()), + b3BufferInfoCL(shapeBuf->getBufferCL()), + b3BufferInfoCL(constraint->getBufferCL()), + b3BufferInfoCL(m_data->m_solverGPU->m_numConstraints->getBufferCL()), + b3BufferInfoCL(m_data->m_solverGPU->m_offsets->getBufferCL()) #ifdef DEBUG_ME - ,b3BufferInfoCL(&gpuDebugInfo) -#endif //DEBUG_ME + , + b3BufferInfoCL(&gpuDebugInfo) +#endif //DEBUG_ME }; - b3LauncherCL launcher( m_data->m_queue, m_data->m_solveFrictionKernel,"m_solveFrictionKernel" ); - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); - launcher.setBuffer( m_data->m_solverGPU->m_batchSizes.getBufferCL()); + b3LauncherCL launcher(m_data->m_queue, m_data->m_solveFrictionKernel, "m_solveFrictionKernel"); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); + launcher.setBuffer(m_data->m_solverGPU->m_batchSizes.getBufferCL()); //launcher.setConst( cdata.x ); - launcher.setConst( cdata.y ); - launcher.setConst( cdata.z ); + launcher.setConst(cdata.y); + launcher.setConst(cdata.z); - b3Int4 nSplit; + b3Int4 nSplit; nSplit.x = B3_SOLVER_N_SPLIT_X; nSplit.y = B3_SOLVER_N_SPLIT_Y; nSplit.z = B3_SOLVER_N_SPLIT_Z; - launcher.setConst( nSplit ); - - launcher.launch1D( 64*nn/B3_SOLVER_N_BATCHES, 64 ); + launcher.setConst(nSplit); + + launcher.launch1D(64 * nn / B3_SOLVER_N_BATCHES, 64); } } clFinish(m_data->m_queue); - } #ifdef DEBUG_ME delete[] debugInfo; -#endif //DEBUG_ME +#endif //DEBUG_ME } - - } - - - - - - - - - - -static bool sortfnc(const b3SortData& a,const b3SortData& b) +static bool sortfnc(const b3SortData& a, const b3SortData& b) { - return (a.m_key<b.m_key); + return (a.m_key < b.m_key); } static bool b3ContactCmp(const b3Contact4& p, const b3Contact4& q) { - return ((p.m_bodyAPtrAndSignBit<q.m_bodyAPtrAndSignBit) || - ((p.m_bodyAPtrAndSignBit==q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit<q.m_bodyBPtrAndSignBit)) || - ((p.m_bodyAPtrAndSignBit==q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit==q.m_bodyBPtrAndSignBit) && p.m_childIndexA<q.m_childIndexA ) || - ((p.m_bodyAPtrAndSignBit==q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit==q.m_bodyBPtrAndSignBit) && p.m_childIndexA<q.m_childIndexA ) || - ((p.m_bodyAPtrAndSignBit==q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit==q.m_bodyBPtrAndSignBit) && p.m_childIndexA==q.m_childIndexA && p.m_childIndexB<q.m_childIndexB) - ); + return ((p.m_bodyAPtrAndSignBit < q.m_bodyAPtrAndSignBit) || + ((p.m_bodyAPtrAndSignBit == q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit < q.m_bodyBPtrAndSignBit)) || + ((p.m_bodyAPtrAndSignBit == q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit == q.m_bodyBPtrAndSignBit) && p.m_childIndexA < q.m_childIndexA) || + ((p.m_bodyAPtrAndSignBit == q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit == q.m_bodyBPtrAndSignBit) && p.m_childIndexA < q.m_childIndexA) || + ((p.m_bodyAPtrAndSignBit == q.m_bodyAPtrAndSignBit) && (p.m_bodyBPtrAndSignBit == q.m_bodyBPtrAndSignBit) && p.m_childIndexA == q.m_childIndexA && p.m_childIndexB < q.m_childIndexB)); } - - - - - - - - - - #define USE_SPATIAL_BATCHING 1 #define USE_4x4_GRID 1 #ifndef USE_SPATIAL_BATCHING -static const int gridTable4x4[] = -{ - 0,1,17,16, - 1,2,18,19, - 17,18,32,3, - 16,19,3,34 -}; -static const int gridTable8x8[] = -{ - 0, 2, 3, 16, 17, 18, 19, 1, - 66, 64, 80, 67, 82, 81, 65, 83, - 131,144,128,130,147,129,145,146, - 208,195,194,192,193,211,210,209, - 21, 22, 23, 5, 4, 6, 7, 20, - 86, 85, 69, 87, 70, 68, 84, 71, - 151,133,149,150,135,148,132,134, - 197,27,214,213,212,199,198,196 - -}; +static const int gridTable4x4[] = + { + 0, 1, 17, 16, + 1, 2, 18, 19, + 17, 18, 32, 3, + 16, 19, 3, 34}; +static const int gridTable8x8[] = + { + 0, 2, 3, 16, 17, 18, 19, 1, + 66, 64, 80, 67, 82, 81, 65, 83, + 131, 144, 128, 130, 147, 129, 145, 146, + 208, 195, 194, 192, 193, 211, 210, 209, + 21, 22, 23, 5, 4, 6, 7, 20, + 86, 85, 69, 87, 70, 68, 84, 71, + 151, 133, 149, 150, 135, 148, 132, 134, + 197, 27, 214, 213, 212, 199, 198, 196 +}; #endif - -void SetSortDataCPU(b3Contact4* gContact, b3RigidBodyData* gBodies, b3SortData* gSortDataOut, int nContacts,float scale,const b3Int4& nSplit,int staticIdx) +void SetSortDataCPU(b3Contact4* gContact, b3RigidBodyData* gBodies, b3SortData* gSortDataOut, int nContacts, float scale, const b3Int4& nSplit, int staticIdx) { - for (int gIdx=0;gIdx<nContacts;gIdx++) + for (int gIdx = 0; gIdx < nContacts; gIdx++) { - if( gIdx < nContacts ) + if (gIdx < nContacts) { - int aPtrAndSignBit = gContact[gIdx].m_bodyAPtrAndSignBit; - int bPtrAndSignBit = gContact[gIdx].m_bodyBPtrAndSignBit; + int aPtrAndSignBit = gContact[gIdx].m_bodyAPtrAndSignBit; + int bPtrAndSignBit = gContact[gIdx].m_bodyBPtrAndSignBit; - int aIdx = abs(aPtrAndSignBit ); + int aIdx = abs(aPtrAndSignBit); int bIdx = abs(bPtrAndSignBit); - bool aStatic = (aPtrAndSignBit<0) ||(aPtrAndSignBit==staticIdx); + bool aStatic = (aPtrAndSignBit < 0) || (aPtrAndSignBit == staticIdx); - #if USE_SPATIAL_BATCHING - int idx = (aStatic)? bIdx: aIdx; +#if USE_SPATIAL_BATCHING + int idx = (aStatic) ? bIdx : aIdx; b3Vector3 p = gBodies[idx].m_pos; - int xIdx = (int)((p.x-((p.x<0.f)?1.f:0.f))*scale) & (nSplit.x-1); - int yIdx = (int)((p.y-((p.y<0.f)?1.f:0.f))*scale) & (nSplit.y-1); - int zIdx = (int)((p.z-((p.z<0.f)?1.f:0.f))*scale) & (nSplit.z-1); - - int newIndex = (xIdx+yIdx*nSplit.x+zIdx*nSplit.x*nSplit.y); - - #else//USE_SPATIAL_BATCHING - bool bStatic = (bPtrAndSignBit<0) ||(bPtrAndSignBit==staticIdx); - - #if USE_4x4_GRID - int aa = aIdx&3; - int bb = bIdx&3; + int xIdx = (int)((p.x - ((p.x < 0.f) ? 1.f : 0.f)) * scale) & (nSplit.x - 1); + int yIdx = (int)((p.y - ((p.y < 0.f) ? 1.f : 0.f)) * scale) & (nSplit.y - 1); + int zIdx = (int)((p.z - ((p.z < 0.f) ? 1.f : 0.f)) * scale) & (nSplit.z - 1); + + int newIndex = (xIdx + yIdx * nSplit.x + zIdx * nSplit.x * nSplit.y); + +#else //USE_SPATIAL_BATCHING + bool bStatic = (bPtrAndSignBit < 0) || (bPtrAndSignBit == staticIdx); + +#if USE_4x4_GRID + int aa = aIdx & 3; + int bb = bIdx & 3; if (aStatic) aa = bb; if (bStatic) bb = aa; - int gridIndex = aa + bb*4; + int gridIndex = aa + bb * 4; int newIndex = gridTable4x4[gridIndex]; - #else//USE_4x4_GRID - int aa = aIdx&7; - int bb = bIdx&7; +#else //USE_4x4_GRID + int aa = aIdx & 7; + int bb = bIdx & 7; if (aStatic) aa = bb; if (bStatic) bb = aa; - int gridIndex = aa + bb*8; + int gridIndex = aa + bb * 8; int newIndex = gridTable8x8[gridIndex]; - #endif//USE_4x4_GRID - #endif//USE_SPATIAL_BATCHING - +#endif //USE_4x4_GRID +#endif //USE_SPATIAL_BATCHING gSortDataOut[gIdx].x = newIndex; gSortDataOut[gIdx].y = gIdx; @@ -650,17 +565,12 @@ void SetSortDataCPU(b3Contact4* gContact, b3RigidBodyData* gBodies, b3SortData* } } - - - - - void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const b3Config& config, int static0Index) { B3_PROFILE("solveContacts"); - m_data->m_bodyBufferGPU->setFromOpenCLBuffer(bodyBuf,numBodies); - m_data->m_inertiaBufferGPU->setFromOpenCLBuffer(inertiaBuf,numBodies); - m_data->m_pBufContactOutGPU->setFromOpenCLBuffer(contactBuf,numContacts); + m_data->m_bodyBufferGPU->setFromOpenCLBuffer(bodyBuf, numBodies); + m_data->m_inertiaBufferGPU->setFromOpenCLBuffer(inertiaBuf, numBodies); + m_data->m_pBufContactOutGPU->setFromOpenCLBuffer(contactBuf, numContacts); if (optionalSortContactsDeterminism) { @@ -671,61 +581,61 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem m_data->m_pBufContactOutGPUCopy->resize(numContacts); m_data->m_contactKeyValues->resize(numContacts); - m_data->m_pBufContactOutGPU->copyToCL(m_data->m_pBufContactOutGPUCopy->getBufferCL(),numContacts,0,0); + m_data->m_pBufContactOutGPU->copyToCL(m_data->m_pBufContactOutGPUCopy->getBufferCL(), numContacts, 0, 0); { - b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataChildShapeBKernel,"m_setDeterminismSortDataChildShapeBKernel"); + b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataChildShapeBKernel, "m_setDeterminismSortDataChildShapeBKernel"); launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); launcher.setConst(numContacts); - launcher.launch1D( numContacts, 64 ); + launcher.launch1D(numContacts, 64); } m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); { - b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataChildShapeAKernel,"m_setDeterminismSortDataChildShapeAKernel"); + b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataChildShapeAKernel, "m_setDeterminismSortDataChildShapeAKernel"); launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); launcher.setConst(numContacts); - launcher.launch1D( numContacts, 64 ); + launcher.launch1D(numContacts, 64); } m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); { - b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataBodyBKernel,"m_setDeterminismSortDataBodyBKernel"); + b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataBodyBKernel, "m_setDeterminismSortDataBodyBKernel"); launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); launcher.setConst(numContacts); - launcher.launch1D( numContacts, 64 ); + launcher.launch1D(numContacts, 64); } - + m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); - + { - b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataBodyAKernel,"m_setDeterminismSortDataBodyAKernel"); + b3LauncherCL launcher(m_data->m_queue, m_data->m_setDeterminismSortDataBodyAKernel, "m_setDeterminismSortDataBodyAKernel"); launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); launcher.setConst(numContacts); - launcher.launch1D( numContacts, 64 ); + launcher.launch1D(numContacts, 64); } m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); { B3_PROFILE("gpu reorderContactKernel (determinism)"); - + b3Int4 cdata; cdata.x = numContacts; - + //b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_data->m_pBufContactOutGPU->getBufferCL() ), b3BufferInfoCL( m_data->m_solverGPU->m_contactBuffer2->getBufferCL()) // , b3BufferInfoCL( m_data->m_solverGPU->m_sortDataBuffer->getBufferCL()) }; - b3LauncherCL launcher(m_data->m_queue,m_data->m_solverGPU->m_reorderContactKernel,"m_reorderContactKernel"); + b3LauncherCL launcher(m_data->m_queue, m_data->m_solverGPU->m_reorderContactKernel, "m_reorderContactKernel"); launcher.setBuffer(m_data->m_pBufContactOutGPUCopy->getBufferCL()); launcher.setBuffer(m_data->m_pBufContactOutGPU->getBufferCL()); launcher.setBuffer(m_data->m_contactKeyValues->getBufferCL()); - launcher.setConst( cdata ); - launcher.launch1D( numContacts, 64 ); - } - - } else + launcher.setConst(cdata); + launcher.launch1D(numContacts, 64); + } + } + else { B3_PROFILE("CPU Sort contact constraints (determinism)"); b3AlignedObjectArray<b3Contact4> cpuConstraints; @@ -735,96 +645,80 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem { cpuConstraints.quickSort(b3ContactCmp); - for (int i=0;i<cpuConstraints.size();i++) + for (int i = 0; i < cpuConstraints.size(); i++) { cpuConstraints[i].m_batchIdx = i; } } m_data->m_pBufContactOutGPU->copyFromHost(cpuConstraints); - if (m_debugOutput==100) + if (m_debugOutput == 100) { - for (int i=0;i<cpuConstraints.size();i++) + for (int i = 0; i < cpuConstraints.size(); i++) { - printf("c[%d].m_bodyA = %d, m_bodyB = %d, batchId = %d\n",i,cpuConstraints[i].m_bodyAPtrAndSignBit,cpuConstraints[i].m_bodyBPtrAndSignBit, cpuConstraints[i].m_batchIdx); + printf("c[%d].m_bodyA = %d, m_bodyB = %d, batchId = %d\n", i, cpuConstraints[i].m_bodyAPtrAndSignBit, cpuConstraints[i].m_bodyBPtrAndSignBit, cpuConstraints[i].m_batchIdx); } } m_debugOutput++; } } - - - int nContactOut = m_data->m_pBufContactOutGPU->size(); bool useSolver = true; - - - if (useSolver) - { - float dt=1./60.; - b3ConstraintCfg csCfg( dt ); - csCfg.m_enableParallelSolve = true; - csCfg.m_batchCellSize = 6; - csCfg.m_staticIdx = static0Index; - - - b3OpenCLArray<b3RigidBodyData>* bodyBuf = m_data->m_bodyBufferGPU; - - void* additionalData = 0;//m_data->m_frictionCGPU; - const b3OpenCLArray<b3InertiaData>* shapeBuf = m_data->m_inertiaBufferGPU; - b3OpenCLArray<b3GpuConstraint4>* contactConstraintOut = m_data->m_contactCGPU; - int nContacts = nContactOut; - - + + if (useSolver) + { + float dt = 1. / 60.; + b3ConstraintCfg csCfg(dt); + csCfg.m_enableParallelSolve = true; + csCfg.m_batchCellSize = 6; + csCfg.m_staticIdx = static0Index; + + b3OpenCLArray<b3RigidBodyData>* bodyBuf = m_data->m_bodyBufferGPU; + + void* additionalData = 0; //m_data->m_frictionCGPU; + const b3OpenCLArray<b3InertiaData>* shapeBuf = m_data->m_inertiaBufferGPU; + b3OpenCLArray<b3GpuConstraint4>* contactConstraintOut = m_data->m_contactCGPU; + int nContacts = nContactOut; + int maxNumBatches = 0; - + if (!gUseLargeBatches) - { - - if( m_data->m_solverGPU->m_contactBuffer2) - { - m_data->m_solverGPU->m_contactBuffer2->resize(nContacts); - } - - if( m_data->m_solverGPU->m_contactBuffer2 == 0 ) - { - m_data->m_solverGPU->m_contactBuffer2 = new b3OpenCLArray<b3Contact4>(m_data->m_context,m_data->m_queue, nContacts ); - m_data->m_solverGPU->m_contactBuffer2->resize(nContacts); - } - - //clFinish(m_data->m_queue); - - - + { + if (m_data->m_solverGPU->m_contactBuffer2) { - B3_PROFILE("batching"); - //@todo: just reserve it, without copy of original contact (unless we use warmstarting) + m_data->m_solverGPU->m_contactBuffer2->resize(nContacts); + } + if (m_data->m_solverGPU->m_contactBuffer2 == 0) + { + m_data->m_solverGPU->m_contactBuffer2 = new b3OpenCLArray<b3Contact4>(m_data->m_context, m_data->m_queue, nContacts); + m_data->m_solverGPU->m_contactBuffer2->resize(nContacts); + } + //clFinish(m_data->m_queue); - //const b3OpenCLArray<b3RigidBodyData>* bodyNative = bodyBuf; + { + B3_PROFILE("batching"); + //@todo: just reserve it, without copy of original contact (unless we use warmstarting) + //const b3OpenCLArray<b3RigidBodyData>* bodyNative = bodyBuf; { - //b3OpenCLArray<b3RigidBodyData>* bodyNative = b3OpenCLArrayUtils::map<adl::TYPE_CL, true>( data->m_device, bodyBuf ); //b3OpenCLArray<b3Contact4>* contactNative = b3OpenCLArrayUtils::map<adl::TYPE_CL, true>( data->m_device, contactsIn ); - const int sortAlignment = 512; // todo. get this out of sort - if( csCfg.m_enableParallelSolve ) + const int sortAlignment = 512; // todo. get this out of sort + if (csCfg.m_enableParallelSolve) { - - - int sortSize = B3NEXTMULTIPLEOF( nContacts, sortAlignment ); + int sortSize = B3NEXTMULTIPLEOF(nContacts, sortAlignment); b3OpenCLArray<unsigned int>* countsNative = m_data->m_solverGPU->m_numConstraints; b3OpenCLArray<unsigned int>* offsetsNative = m_data->m_solverGPU->m_offsets; - if (!gCpuSetSortData) - { // 2. set cell idx + { // 2. set cell idx B3_PROFILE("GPU set cell idx"); struct CB { @@ -834,29 +728,28 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem b3Int4 m_nSplit; }; - b3Assert( sortSize%64 == 0 ); + b3Assert(sortSize % 64 == 0); CB cdata; cdata.m_nContacts = nContacts; cdata.m_staticIdx = csCfg.m_staticIdx; - cdata.m_scale = 1.f/csCfg.m_batchCellSize; + cdata.m_scale = 1.f / csCfg.m_batchCellSize; cdata.m_nSplit.x = B3_SOLVER_N_SPLIT_X; cdata.m_nSplit.y = B3_SOLVER_N_SPLIT_Y; cdata.m_nSplit.z = B3_SOLVER_N_SPLIT_Z; m_data->m_solverGPU->m_sortDataBuffer->resize(nContacts); - - b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_data->m_pBufContactOutGPU->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL()), b3BufferInfoCL( m_data->m_solverGPU->m_sortDataBuffer->getBufferCL()) }; - b3LauncherCL launcher(m_data->m_queue, m_data->m_solverGPU->m_setSortDataKernel,"m_setSortDataKernel" ); - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); - launcher.setConst( cdata.m_nContacts ); - launcher.setConst( cdata.m_scale ); + b3BufferInfoCL bInfo[] = {b3BufferInfoCL(m_data->m_pBufContactOutGPU->getBufferCL()), b3BufferInfoCL(bodyBuf->getBufferCL()), b3BufferInfoCL(m_data->m_solverGPU->m_sortDataBuffer->getBufferCL())}; + b3LauncherCL launcher(m_data->m_queue, m_data->m_solverGPU->m_setSortDataKernel, "m_setSortDataKernel"); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); + launcher.setConst(cdata.m_nContacts); + launcher.setConst(cdata.m_scale); launcher.setConst(cdata.m_nSplit); launcher.setConst(cdata.m_staticIdx); - - launcher.launch1D( sortSize, 64 ); - } else + launcher.launch1D(sortSize, 64); + } + else { m_data->m_solverGPU->m_sortDataBuffer->resize(nContacts); b3AlignedObjectArray<b3SortData> sortDataCPU; @@ -866,22 +759,19 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem m_data->m_pBufContactOutGPU->copyToHost(contactCPU); b3AlignedObjectArray<b3RigidBodyData> bodiesCPU; bodyBuf->copyToHost(bodiesCPU); - float scale = 1.f/csCfg.m_batchCellSize; + float scale = 1.f / csCfg.m_batchCellSize; b3Int4 nSplit; nSplit.x = B3_SOLVER_N_SPLIT_X; nSplit.y = B3_SOLVER_N_SPLIT_Y; nSplit.z = B3_SOLVER_N_SPLIT_Z; - SetSortDataCPU(&contactCPU[0], &bodiesCPU[0], &sortDataCPU[0], nContacts,scale,nSplit,csCfg.m_staticIdx); - + SetSortDataCPU(&contactCPU[0], &bodiesCPU[0], &sortDataCPU[0], nContacts, scale, nSplit, csCfg.m_staticIdx); m_data->m_solverGPU->m_sortDataBuffer->copyFromHost(sortDataCPU); } - - if (!gCpuRadixSort) - { // 3. sort by cell idx + { // 3. sort by cell idx B3_PROFILE("gpuRadixSort"); //int n = B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT; //int sortBit = 32; @@ -891,10 +781,8 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem //adl::RadixSort32<adl::TYPE_CL>::execute( data->m_sort32, *data->m_sortDataBuffer, sortSize ); b3OpenCLArray<b3SortData>& keyValuesInOut = *(m_data->m_solverGPU->m_sortDataBuffer); this->m_data->m_solverGPU->m_sort32->execute(keyValuesInOut); - - - - } else + } + else { b3OpenCLArray<b3SortData>& keyValuesInOut = *(m_data->m_solverGPU->m_sortDataBuffer); b3AlignedObjectArray<b3SortData> hostValues; @@ -903,7 +791,6 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem keyValuesInOut.copyFromHost(hostValues); } - if (gUseScanHost) { // 4. find entries @@ -914,13 +801,11 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem b3AlignedObjectArray<b3SortData> sortDataHost; m_data->m_solverGPU->m_sortDataBuffer->copyToHost(sortDataHost); - //m_data->m_solverGPU->m_search->executeHost(*m_data->m_solverGPU->m_sortDataBuffer,nContacts,*countsNative,B3_SOLVER_N_CELLS,b3BoundSearchCL::COUNT); - m_data->m_solverGPU->m_search->executeHost(sortDataHost,nContacts,countsHost,B3_SOLVER_N_CELLS,b3BoundSearchCL::COUNT); + m_data->m_solverGPU->m_search->executeHost(sortDataHost, nContacts, countsHost, B3_SOLVER_N_CELLS, b3BoundSearchCL::COUNT); countsNative->copyFromHost(countsHost); - //adl::BoundSearch<adl::TYPE_CL>::execute( data->m_search, *data->m_sortDataBuffer, nContacts, *countsNative, // B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT, adl::BoundSearchBase::COUNT ); @@ -929,24 +814,21 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem b3AlignedObjectArray<unsigned int> offsetsHost; offsetsHost.resize(offsetsNative->size()); - - m_data->m_solverGPU->m_scan->executeHost(countsHost,offsetsHost, B3_SOLVER_N_CELLS);//,&sum ); + m_data->m_solverGPU->m_scan->executeHost(countsHost, offsetsHost, B3_SOLVER_N_CELLS); //,&sum ); offsetsNative->copyFromHost(offsetsHost); //printf("sum = %d\n",sum); - } else + } + else { // 4. find entries B3_PROFILE("gpuBoundSearch"); - m_data->m_solverGPU->m_search->execute(*m_data->m_solverGPU->m_sortDataBuffer,nContacts,*countsNative,B3_SOLVER_N_CELLS,b3BoundSearchCL::COUNT); - m_data->m_solverGPU->m_scan->execute(*countsNative,*offsetsNative, B3_SOLVER_N_CELLS);//,&sum ); - } - - - + m_data->m_solverGPU->m_search->execute(*m_data->m_solverGPU->m_sortDataBuffer, nContacts, *countsNative, B3_SOLVER_N_CELLS, b3BoundSearchCL::COUNT); + m_data->m_solverGPU->m_scan->execute(*countsNative, *offsetsNative, B3_SOLVER_N_CELLS); //,&sum ); + } if (nContacts) - { // 5. sort constraints by cellIdx + { // 5. sort constraints by cellIdx if (gReorderContactsOnCpu) { B3_PROFILE("cpu m_reorderContactKernel"); @@ -956,7 +838,7 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem b3AlignedObjectArray<b3Contact4> outContacts; m_data->m_pBufContactOutGPU->copyToHost(inContacts); outContacts.resize(inContacts.size()); - for (int i=0;i<nContacts;i++) + for (int i = 0; i < nContacts; i++) { int srcIdx = sortDataHost[i].y; outContacts[i] = inContacts[srcIdx]; @@ -974,30 +856,25 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem " }\n" "}\n" */ - } else + } + else { B3_PROFILE("gpu m_reorderContactKernel"); b3Int4 cdata; cdata.x = nContacts; - b3BufferInfoCL bInfo[] = { - b3BufferInfoCL( m_data->m_pBufContactOutGPU->getBufferCL() ), - b3BufferInfoCL( m_data->m_solverGPU->m_contactBuffer2->getBufferCL()) - , b3BufferInfoCL( m_data->m_solverGPU->m_sortDataBuffer->getBufferCL()) }; + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL(m_data->m_pBufContactOutGPU->getBufferCL()), + b3BufferInfoCL(m_data->m_solverGPU->m_contactBuffer2->getBufferCL()), b3BufferInfoCL(m_data->m_solverGPU->m_sortDataBuffer->getBufferCL())}; - b3LauncherCL launcher(m_data->m_queue,m_data->m_solverGPU->m_reorderContactKernel,"m_reorderContactKernel"); - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); - launcher.setConst( cdata ); - launcher.launch1D( nContacts, 64 ); + b3LauncherCL launcher(m_data->m_queue, m_data->m_solverGPU->m_reorderContactKernel, "m_reorderContactKernel"); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); + launcher.setConst(cdata); + launcher.launch1D(nContacts, 64); } } - - - - } - } //clFinish(m_data->m_queue); @@ -1008,48 +885,46 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem // printf(",,,\n"); // } - if (nContacts) { - if (gUseCpuCopyConstraints) { - for (int i=0;i<nContacts;i++) + for (int i = 0; i < nContacts; i++) { m_data->m_pBufContactOutGPU->copyFromOpenCLArray(*m_data->m_solverGPU->m_contactBuffer2); - // m_data->m_solverGPU->m_contactBuffer2->getBufferCL(); - // m_data->m_pBufContactOutGPU->getBufferCL() + // m_data->m_solverGPU->m_contactBuffer2->getBufferCL(); + // m_data->m_pBufContactOutGPU->getBufferCL() } - - } else + } + else { B3_PROFILE("gpu m_copyConstraintKernel"); - b3Int4 cdata; cdata.x = nContacts; - b3BufferInfoCL bInfo[] = { - b3BufferInfoCL( m_data->m_solverGPU->m_contactBuffer2->getBufferCL() ), - b3BufferInfoCL( m_data->m_pBufContactOutGPU->getBufferCL() ) - }; - - b3LauncherCL launcher(m_data->m_queue, m_data->m_solverGPU->m_copyConstraintKernel,"m_copyConstraintKernel" ); - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); - launcher.setConst( cdata ); - launcher.launch1D( nContacts, 64 ); + b3Int4 cdata; + cdata.x = nContacts; + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL(m_data->m_solverGPU->m_contactBuffer2->getBufferCL()), + b3BufferInfoCL(m_data->m_pBufContactOutGPU->getBufferCL())}; + + b3LauncherCL launcher(m_data->m_queue, m_data->m_solverGPU->m_copyConstraintKernel, "m_copyConstraintKernel"); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); + launcher.setConst(cdata); + launcher.launch1D(nContacts, 64); //we use the clFinish for proper benchmark/profile clFinish(m_data->m_queue); } } - -// bool compareGPU = false; + // bool compareGPU = false; if (nContacts) { if (!gCpuBatchContacts) { B3_PROFILE("gpu batchContacts"); - maxNumBatches = 250;//250; - m_data->m_solverGPU->batchContacts( m_data->m_pBufContactOutGPU, nContacts, m_data->m_solverGPU->m_numConstraints, m_data->m_solverGPU->m_offsets, csCfg.m_staticIdx ); + maxNumBatches = 250; //250; + m_data->m_solverGPU->batchContacts(m_data->m_pBufContactOutGPU, nContacts, m_data->m_solverGPU->m_numConstraints, m_data->m_solverGPU->m_offsets, csCfg.m_staticIdx); clFinish(m_data->m_queue); - } else + } + else { B3_PROFILE("cpu batchContacts"); static b3AlignedObjectArray<b3Contact4> cpuContacts; @@ -1070,45 +945,43 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem offsetsNative->copyToHost(offsetsNativeHost); } - - int numNonzeroGrid=0; + int numNonzeroGrid = 0; if (gUseLargeBatches) { m_data->m_batchSizes.resize(B3_MAX_NUM_BATCHES); int totalNumConstraints = cpuContacts.size(); //int simdWidth =numBodies+1;//-1;//64;//-1;//32; - int numBatches = sortConstraintByBatch3( &cpuContacts[0], totalNumConstraints, totalNumConstraints+1,csCfg.m_staticIdx ,numBodies,&m_data->m_batchSizes[0]); // on GPU - maxNumBatches = b3Max(numBatches,maxNumBatches); + int numBatches = sortConstraintByBatch3(&cpuContacts[0], totalNumConstraints, totalNumConstraints + 1, csCfg.m_staticIdx, numBodies, &m_data->m_batchSizes[0]); // on GPU + maxNumBatches = b3Max(numBatches, maxNumBatches); static int globalMaxBatch = 0; - if (maxNumBatches>globalMaxBatch ) + if (maxNumBatches > globalMaxBatch) { - globalMaxBatch = maxNumBatches; - b3Printf("maxNumBatches = %d\n",maxNumBatches); + globalMaxBatch = maxNumBatches; + b3Printf("maxNumBatches = %d\n", maxNumBatches); } - - } else + } + else { - m_data->m_batchSizes.resize(B3_SOLVER_N_CELLS*B3_MAX_NUM_BATCHES); + m_data->m_batchSizes.resize(B3_SOLVER_N_CELLS * B3_MAX_NUM_BATCHES); B3_PROFILE("cpu batch grid"); - for(int i=0; i<B3_SOLVER_N_CELLS; i++) + for (int i = 0; i < B3_SOLVER_N_CELLS; i++) { int n = (nNativeHost)[i]; int offset = (offsetsNativeHost)[i]; - if( n ) + if (n) { numNonzeroGrid++; - int simdWidth =numBodies+1;//-1;//64;//-1;//32; - int numBatches = sortConstraintByBatch3( &cpuContacts[0]+offset, n, simdWidth,csCfg.m_staticIdx ,numBodies,&m_data->m_batchSizes[i*B3_MAX_NUM_BATCHES]); // on GPU - maxNumBatches = b3Max(numBatches,maxNumBatches); + int simdWidth = numBodies + 1; //-1;//64;//-1;//32; + int numBatches = sortConstraintByBatch3(&cpuContacts[0] + offset, n, simdWidth, csCfg.m_staticIdx, numBodies, &m_data->m_batchSizes[i * B3_MAX_NUM_BATCHES]); // on GPU + maxNumBatches = b3Max(numBatches, maxNumBatches); static int globalMaxBatch = 0; - if (maxNumBatches>globalMaxBatch ) + if (maxNumBatches > globalMaxBatch) { - globalMaxBatch = maxNumBatches; - b3Printf("maxNumBatches = %d\n",maxNumBatches); + globalMaxBatch = maxNumBatches; + b3Printf("maxNumBatches = %d\n", maxNumBatches); } //we use the clFinish for proper benchmark/profile - } } //clFinish(m_data->m_queue); @@ -1117,22 +990,12 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem B3_PROFILE("m_contactBuffer->copyFromHost"); m_data->m_solverGPU->m_contactBuffer2->copyFromHost((b3AlignedObjectArray<b3Contact4>&)cpuContacts); } - - } - + } } + } + } - - - - - } - - - } - - - //printf("maxNumBatches = %d\n", maxNumBatches); + //printf("maxNumBatches = %d\n", maxNumBatches); if (gUseLargeBatches) { @@ -1140,58 +1003,52 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem { B3_PROFILE("cpu batchContacts"); static b3AlignedObjectArray<b3Contact4> cpuContacts; -// b3OpenCLArray<b3Contact4>* contactsIn = m_data->m_solverGPU->m_contactBuffer2; + // b3OpenCLArray<b3Contact4>* contactsIn = m_data->m_solverGPU->m_contactBuffer2; { B3_PROFILE("copyToHost"); m_data->m_pBufContactOutGPU->copyToHost(cpuContacts); } -// b3OpenCLArray<unsigned int>* countsNative = m_data->m_solverGPU->m_numConstraints; -// b3OpenCLArray<unsigned int>* offsetsNative = m_data->m_solverGPU->m_offsets; - + // b3OpenCLArray<unsigned int>* countsNative = m_data->m_solverGPU->m_numConstraints; + // b3OpenCLArray<unsigned int>* offsetsNative = m_data->m_solverGPU->m_offsets; - -// int numNonzeroGrid=0; + // int numNonzeroGrid=0; { m_data->m_batchSizes.resize(B3_MAX_NUM_BATCHES); int totalNumConstraints = cpuContacts.size(); - // int simdWidth =numBodies+1;//-1;//64;//-1;//32; - int numBatches = sortConstraintByBatch3( &cpuContacts[0], totalNumConstraints, totalNumConstraints+1,csCfg.m_staticIdx ,numBodies,&m_data->m_batchSizes[0]); // on GPU - maxNumBatches = b3Max(numBatches,maxNumBatches); + // int simdWidth =numBodies+1;//-1;//64;//-1;//32; + int numBatches = sortConstraintByBatch3(&cpuContacts[0], totalNumConstraints, totalNumConstraints + 1, csCfg.m_staticIdx, numBodies, &m_data->m_batchSizes[0]); // on GPU + maxNumBatches = b3Max(numBatches, maxNumBatches); static int globalMaxBatch = 0; - if (maxNumBatches>globalMaxBatch ) + if (maxNumBatches > globalMaxBatch) { - globalMaxBatch = maxNumBatches; - b3Printf("maxNumBatches = %d\n",maxNumBatches); + globalMaxBatch = maxNumBatches; + b3Printf("maxNumBatches = %d\n", maxNumBatches); } - } { B3_PROFILE("m_contactBuffer->copyFromHost"); m_data->m_solverGPU->m_contactBuffer2->copyFromHost((b3AlignedObjectArray<b3Contact4>&)cpuContacts); } - - } - + } } if (nContacts) { B3_PROFILE("gpu convertToConstraints"); - m_data->m_solverGPU->convertToConstraints( bodyBuf, - shapeBuf, m_data->m_solverGPU->m_contactBuffer2, - contactConstraintOut, - additionalData, nContacts, - (b3SolverBase::ConstraintCfg&) csCfg ); + m_data->m_solverGPU->convertToConstraints(bodyBuf, + shapeBuf, m_data->m_solverGPU->m_contactBuffer2, + contactConstraintOut, + additionalData, nContacts, + (b3SolverBase::ConstraintCfg&)csCfg); clFinish(m_data->m_queue); } - if (1) { int numIter = 4; - m_data->m_solverGPU->m_nIterations = numIter;//10 + m_data->m_solverGPU->m_nIterations = numIter; //10 if (!gCpuSolveConstraint) { B3_PROFILE("GPU solveContactConstraint"); @@ -1208,32 +1065,30 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem if (gUseLargeBatches) { - solveContactConstraintBatchSizes(m_data->m_bodyBufferGPU, - m_data->m_inertiaBufferGPU, - m_data->m_contactCGPU,0, - nContactOut , - maxNumBatches,numIter,&m_data->m_batchSizes); - } else + solveContactConstraintBatchSizes(m_data->m_bodyBufferGPU, + m_data->m_inertiaBufferGPU, + m_data->m_contactCGPU, 0, + nContactOut, + maxNumBatches, numIter, &m_data->m_batchSizes); + } + else { solveContactConstraint( - m_data->m_bodyBufferGPU, + m_data->m_bodyBufferGPU, m_data->m_inertiaBufferGPU, - m_data->m_contactCGPU,0, - nContactOut , - maxNumBatches,numIter,&m_data->m_batchSizes);//m_data->m_batchSizesGpu); + m_data->m_contactCGPU, 0, + nContactOut, + maxNumBatches, numIter, &m_data->m_batchSizes); //m_data->m_batchSizesGpu); } } else { B3_PROFILE("Host solveContactConstraint"); - m_data->m_solverGPU->solveContactConstraintHost(m_data->m_bodyBufferGPU, m_data->m_inertiaBufferGPU, m_data->m_contactCGPU,0, nContactOut ,maxNumBatches,&m_data->m_batchSizes); + m_data->m_solverGPU->solveContactConstraintHost(m_data->m_bodyBufferGPU, m_data->m_inertiaBufferGPU, m_data->m_contactCGPU, 0, nContactOut, maxNumBatches, &m_data->m_batchSizes); } - - - } - - + } + #if 0 if (0) { @@ -1244,114 +1099,96 @@ void b3GpuPgsContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem adl::DeviceUtils::waitForCompletion( m_data->m_deviceCL ); } #endif - - } - + } } - -void b3GpuPgsContactSolver::batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* n, b3OpenCLArray<unsigned int>* offsets, int staticIdx ) +void b3GpuPgsContactSolver::batchContacts(b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* n, b3OpenCLArray<unsigned int>* offsets, int staticIdx) { } - - - - - - - - - - b3AlignedObjectArray<unsigned int> idxBuffer; b3AlignedObjectArray<b3SortData> sortData; b3AlignedObjectArray<b3Contact4> old; - -inline int b3GpuPgsContactSolver::sortConstraintByBatch( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies) +inline int b3GpuPgsContactSolver::sortConstraintByBatch(b3Contact4* cs, int n, int simdWidth, int staticIdx, int numBodies) { - B3_PROFILE("sortConstraintByBatch"); int numIter = 0; - + sortData.resize(n); idxBuffer.resize(n); old.resize(n); - + unsigned int* idxSrc = &idxBuffer[0]; unsigned int* idxDst = &idxBuffer[0]; int nIdxSrc, nIdxDst; - + const int N_FLG = 256; - const int FLG_MASK = N_FLG-1; - unsigned int flg[N_FLG/32]; + const int FLG_MASK = N_FLG - 1; + unsigned int flg[N_FLG / 32]; #if defined(_DEBUG) - for(int i=0; i<n; i++) + for (int i = 0; i < n; i++) cs[i].getBatchIdx() = -1; #endif - for(int i=0; i<n; i++) + for (int i = 0; i < n; i++) idxSrc[i] = i; nIdxSrc = n; - + int batchIdx = 0; - + { B3_PROFILE("cpu batch innerloop"); - while( nIdxSrc ) + while (nIdxSrc) { numIter++; nIdxDst = 0; int nCurrentBatch = 0; - + // clear flag - for(int i=0; i<N_FLG/32; i++) flg[i] = 0; - - for(int i=0; i<nIdxSrc; i++) + for (int i = 0; i < N_FLG / 32; i++) flg[i] = 0; + + for (int i = 0; i < nIdxSrc; i++) { int idx = idxSrc[i]; - - b3Assert( idx < n ); + b3Assert(idx < n); // check if it can go int bodyAS = cs[idx].m_bodyAPtrAndSignBit; int bodyBS = cs[idx].m_bodyBPtrAndSignBit; - - - + int bodyA = abs(bodyAS); int bodyB = abs(bodyBS); - + int aIdx = bodyA & FLG_MASK; int bIdx = bodyB & FLG_MASK; - - unsigned int aUnavailable = flg[ aIdx/32 ] & (1<<(aIdx&31)); - unsigned int bUnavailable = flg[ bIdx/32 ] & (1<<(bIdx&31)); - - bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx; - bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx; - - //use inv_mass! - aUnavailable = !aIsStatic? aUnavailable:0;// - bUnavailable = !bIsStatic? bUnavailable:0; - - if( aUnavailable==0 && bUnavailable==0 ) // ok + + unsigned int aUnavailable = flg[aIdx / 32] & (1 << (aIdx & 31)); + unsigned int bUnavailable = flg[bIdx / 32] & (1 << (bIdx & 31)); + + bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; + bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; + + //use inv_mass! + aUnavailable = !aIsStatic ? aUnavailable : 0; // + bUnavailable = !bIsStatic ? bUnavailable : 0; + + if (aUnavailable == 0 && bUnavailable == 0) // ok { if (!aIsStatic) - flg[ aIdx/32 ] |= (1<<(aIdx&31)); + flg[aIdx / 32] |= (1 << (aIdx & 31)); if (!bIsStatic) - flg[ bIdx/32 ] |= (1<<(bIdx&31)); + flg[bIdx / 32] |= (1 << (bIdx & 31)); cs[idx].getBatchIdx() = batchIdx; sortData[idx].m_key = batchIdx; sortData[idx].m_value = idx; - + { nCurrentBatch++; - if( nCurrentBatch == simdWidth ) + if (nCurrentBatch == simdWidth) { nCurrentBatch = 0; - for(int i=0; i<N_FLG/32; i++) flg[i] = 0; + for (int i = 0; i < N_FLG / 32; i++) flg[i] = 0; } } } @@ -1360,128 +1197,121 @@ inline int b3GpuPgsContactSolver::sortConstraintByBatch( b3Contact4* cs, int n, idxDst[nIdxDst++] = idx; } } - b3Swap( idxSrc, idxDst ); - b3Swap( nIdxSrc, nIdxDst ); - batchIdx ++; + b3Swap(idxSrc, idxDst); + b3Swap(nIdxSrc, nIdxDst); + batchIdx++; } } { B3_PROFILE("quickSort"); sortData.quickSort(sortfnc); } - - + { - B3_PROFILE("reorder"); + B3_PROFILE("reorder"); // reorder - - memcpy( &old[0], cs, sizeof(b3Contact4)*n); - for(int i=0; i<n; i++) + + memcpy(&old[0], cs, sizeof(b3Contact4) * n); + for (int i = 0; i < n; i++) { int idx = sortData[i].m_value; cs[i] = old[idx]; } } - - + #if defined(_DEBUG) - // debugPrintf( "nBatches: %d\n", batchIdx ); - for(int i=0; i<n; i++) - { - b3Assert( cs[i].getBatchIdx() != -1 ); - } + // debugPrintf( "nBatches: %d\n", batchIdx ); + for (int i = 0; i < n; i++) + { + b3Assert(cs[i].getBatchIdx() != -1); + } #endif return batchIdx; } - b3AlignedObjectArray<int> bodyUsed2; -inline int b3GpuPgsContactSolver::sortConstraintByBatch2( b3Contact4* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies) +inline int b3GpuPgsContactSolver::sortConstraintByBatch2(b3Contact4* cs, int numConstraints, int simdWidth, int staticIdx, int numBodies) { - B3_PROFILE("sortConstraintByBatch2"); - - - bodyUsed2.resize(2*simdWidth); + bodyUsed2.resize(2 * simdWidth); - for (int q=0;q<2*simdWidth;q++) - bodyUsed2[q]=0; + for (int q = 0; q < 2 * simdWidth; q++) + bodyUsed2[q] = 0; int curBodyUsed = 0; int numIter = 0; - + m_data->m_sortData.resize(numConstraints); m_data->m_idxBuffer.resize(numConstraints); m_data->m_old.resize(numConstraints); - + unsigned int* idxSrc = &m_data->m_idxBuffer[0]; - + #if defined(_DEBUG) - for(int i=0; i<numConstraints; i++) + for (int i = 0; i < numConstraints; i++) cs[i].getBatchIdx() = -1; #endif - for(int i=0; i<numConstraints; i++) + for (int i = 0; i < numConstraints; i++) idxSrc[i] = i; - + int numValidConstraints = 0; -// int unprocessedConstraintIndex = 0; + // int unprocessedConstraintIndex = 0; int batchIdx = 0; - { B3_PROFILE("cpu batch innerloop"); - - while( numValidConstraints < numConstraints) + + while (numValidConstraints < numConstraints) { numIter++; int nCurrentBatch = 0; // clear flag - for(int i=0; i<curBodyUsed; i++) + for (int i = 0; i < curBodyUsed; i++) bodyUsed2[i] = 0; - curBodyUsed = 0; + curBodyUsed = 0; - for(int i=numValidConstraints; i<numConstraints; i++) + for (int i = numValidConstraints; i < numConstraints; i++) { int idx = idxSrc[i]; - b3Assert( idx < numConstraints ); + b3Assert(idx < numConstraints); // check if it can go int bodyAS = cs[idx].m_bodyAPtrAndSignBit; int bodyBS = cs[idx].m_bodyBPtrAndSignBit; int bodyA = abs(bodyAS); int bodyB = abs(bodyBS); - bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx; - bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx; + bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; + bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; int aUnavailable = 0; int bUnavailable = 0; if (!aIsStatic) { - for (int j=0;j<curBodyUsed;j++) + for (int j = 0; j < curBodyUsed; j++) { if (bodyA == bodyUsed2[j]) { - aUnavailable=1; + aUnavailable = 1; break; } } } if (!aUnavailable) - if (!bIsStatic) - { - for (int j=0;j<curBodyUsed;j++) + if (!bIsStatic) { - if (bodyB == bodyUsed2[j]) + for (int j = 0; j < curBodyUsed; j++) { - bUnavailable=1; - break; + if (bodyB == bodyUsed2[j]) + { + bUnavailable = 1; + break; + } } } - } - - if( aUnavailable==0 && bUnavailable==0 ) // ok + + if (aUnavailable == 0 && bUnavailable == 0) // ok { if (!aIsStatic) { @@ -1496,7 +1326,7 @@ inline int b3GpuPgsContactSolver::sortConstraintByBatch2( b3Contact4* cs, int nu m_data->m_sortData[idx].m_key = batchIdx; m_data->m_sortData[idx].m_value = idx; - if (i!=numValidConstraints) + if (i != numValidConstraints) { b3Swap(idxSrc[i], idxSrc[numValidConstraints]); } @@ -1504,20 +1334,19 @@ inline int b3GpuPgsContactSolver::sortConstraintByBatch2( b3Contact4* cs, int nu numValidConstraints++; { nCurrentBatch++; - if( nCurrentBatch == simdWidth ) + if (nCurrentBatch == simdWidth) { nCurrentBatch = 0; - for(int i=0; i<curBodyUsed; i++) + for (int i = 0; i < curBodyUsed; i++) bodyUsed2[i] = 0; - curBodyUsed = 0; } } } } - - batchIdx ++; + + batchIdx++; } } { @@ -1526,155 +1355,148 @@ inline int b3GpuPgsContactSolver::sortConstraintByBatch2( b3Contact4* cs, int nu } { - B3_PROFILE("reorder"); + B3_PROFILE("reorder"); // reorder - - memcpy( &m_data->m_old[0], cs, sizeof(b3Contact4)*numConstraints); - for(int i=0; i<numConstraints; i++) + memcpy(&m_data->m_old[0], cs, sizeof(b3Contact4) * numConstraints); + + for (int i = 0; i < numConstraints; i++) { b3Assert(m_data->m_sortData[idxSrc[i]].m_value == idxSrc[i]); int idx = m_data->m_sortData[idxSrc[i]].m_value; cs[i] = m_data->m_old[idx]; } } - + #if defined(_DEBUG) - // debugPrintf( "nBatches: %d\n", batchIdx ); - for(int i=0; i<numConstraints; i++) - { - b3Assert( cs[i].getBatchIdx() != -1 ); - } + // debugPrintf( "nBatches: %d\n", batchIdx ); + for (int i = 0; i < numConstraints; i++) + { + b3Assert(cs[i].getBatchIdx() != -1); + } #endif - return batchIdx; } - b3AlignedObjectArray<int> bodyUsed; b3AlignedObjectArray<int> curUsed; - -inline int b3GpuPgsContactSolver::sortConstraintByBatch3( b3Contact4* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies, int* batchSizes) +inline int b3GpuPgsContactSolver::sortConstraintByBatch3(b3Contact4* cs, int numConstraints, int simdWidth, int staticIdx, int numBodies, int* batchSizes) { - B3_PROFILE("sortConstraintByBatch3"); - + static int maxSwaps = 0; int numSwaps = 0; - curUsed.resize(2*simdWidth); + curUsed.resize(2 * simdWidth); static int maxNumConstraints = 0; - if (maxNumConstraints<numConstraints) + if (maxNumConstraints < numConstraints) { maxNumConstraints = numConstraints; //printf("maxNumConstraints = %d\n",maxNumConstraints ); } - int numUsedArray = numBodies/32+1; + int numUsedArray = numBodies / 32 + 1; bodyUsed.resize(numUsedArray); - for (int q=0;q<numUsedArray;q++) - bodyUsed[q]=0; + for (int q = 0; q < numUsedArray; q++) + bodyUsed[q] = 0; - int curBodyUsed = 0; int numIter = 0; - + m_data->m_sortData.resize(0); m_data->m_idxBuffer.resize(0); m_data->m_old.resize(0); - - + #if defined(_DEBUG) - for(int i=0; i<numConstraints; i++) + for (int i = 0; i < numConstraints; i++) cs[i].getBatchIdx() = -1; #endif - + int numValidConstraints = 0; -// int unprocessedConstraintIndex = 0; + // int unprocessedConstraintIndex = 0; int batchIdx = 0; - { B3_PROFILE("cpu batch innerloop"); - - while( numValidConstraints < numConstraints) + + while (numValidConstraints < numConstraints) { numIter++; int nCurrentBatch = 0; batchSizes[batchIdx] = 0; // clear flag - for(int i=0; i<curBodyUsed; i++) - bodyUsed[curUsed[i]/32] = 0; + for (int i = 0; i < curBodyUsed; i++) + bodyUsed[curUsed[i] / 32] = 0; - curBodyUsed = 0; + curBodyUsed = 0; - for(int i=numValidConstraints; i<numConstraints; i++) + for (int i = numValidConstraints; i < numConstraints; i++) { int idx = i; - b3Assert( idx < numConstraints ); + b3Assert(idx < numConstraints); // check if it can go int bodyAS = cs[idx].m_bodyAPtrAndSignBit; int bodyBS = cs[idx].m_bodyBPtrAndSignBit; int bodyA = abs(bodyAS); int bodyB = abs(bodyBS); - bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx; - bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx; + bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx; + bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx; int aUnavailable = 0; int bUnavailable = 0; if (!aIsStatic) { - aUnavailable = bodyUsed[ bodyA/32 ] & (1<<(bodyA&31)); + aUnavailable = bodyUsed[bodyA / 32] & (1 << (bodyA & 31)); } if (!aUnavailable) - if (!bIsStatic) - { - bUnavailable = bodyUsed[ bodyB/32 ] & (1<<(bodyB&31)); - } - - if( aUnavailable==0 && bUnavailable==0 ) // ok + if (!bIsStatic) + { + bUnavailable = bodyUsed[bodyB / 32] & (1 << (bodyB & 31)); + } + + if (aUnavailable == 0 && bUnavailable == 0) // ok { if (!aIsStatic) { - bodyUsed[ bodyA/32 ] |= (1<<(bodyA&31)); - curUsed[curBodyUsed++]=bodyA; + bodyUsed[bodyA / 32] |= (1 << (bodyA & 31)); + curUsed[curBodyUsed++] = bodyA; } if (!bIsStatic) { - bodyUsed[ bodyB/32 ] |= (1<<(bodyB&31)); - curUsed[curBodyUsed++]=bodyB; + bodyUsed[bodyB / 32] |= (1 << (bodyB & 31)); + curUsed[curBodyUsed++] = bodyB; } cs[idx].getBatchIdx() = batchIdx; - if (i!=numValidConstraints) + if (i != numValidConstraints) { - b3Swap(cs[i],cs[numValidConstraints]); + b3Swap(cs[i], cs[numValidConstraints]); numSwaps++; } numValidConstraints++; { nCurrentBatch++; - if( nCurrentBatch == simdWidth ) + if (nCurrentBatch == simdWidth) { batchSizes[batchIdx] += simdWidth; nCurrentBatch = 0; - for(int i=0; i<curBodyUsed; i++) - bodyUsed[curUsed[i]/32] = 0; + for (int i = 0; i < curBodyUsed; i++) + bodyUsed[curUsed[i] / 32] = 0; curBodyUsed = 0; } } } } - if (batchIdx>=B3_MAX_NUM_BATCHES) + if (batchIdx >= B3_MAX_NUM_BATCHES) { b3Error("batchIdx>=B3_MAX_NUM_BATCHES"); b3Assert(0); @@ -1683,26 +1505,25 @@ inline int b3GpuPgsContactSolver::sortConstraintByBatch3( b3Contact4* cs, int nu batchSizes[batchIdx] += nCurrentBatch; - batchIdx ++; - + batchIdx++; } } - + #if defined(_DEBUG) - // debugPrintf( "nBatches: %d\n", batchIdx ); - for(int i=0; i<numConstraints; i++) - { - b3Assert( cs[i].getBatchIdx() != -1 ); - } + // debugPrintf( "nBatches: %d\n", batchIdx ); + for (int i = 0; i < numConstraints; i++) + { + b3Assert(cs[i].getBatchIdx() != -1); + } #endif - batchSizes[batchIdx] =0; - - if (maxSwaps<numSwaps) + batchSizes[batchIdx] = 0; + + if (maxSwaps < numSwaps) { maxSwaps = numSwaps; //printf("maxSwaps = %d\n", maxSwaps); } - + return batchIdx; } diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h index 98e2a5b8c4..6ab7502af3 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h @@ -11,33 +11,27 @@ class b3GpuPgsContactSolver { protected: - int m_debugOutput; - struct b3GpuBatchingPgsSolverInternalData* m_data; + struct b3GpuBatchingPgsSolverInternalData* m_data; + + void batchContacts(b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* n, b3OpenCLArray<unsigned int>* offsets, int staticIdx); - void batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* n, b3OpenCLArray<unsigned int>* offsets, int staticIdx ); - - inline int sortConstraintByBatch( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies); - inline int sortConstraintByBatch2( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies); - inline int sortConstraintByBatch3( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies, int* batchSizes); - + inline int sortConstraintByBatch(b3Contact4* cs, int n, int simdWidth, int staticIdx, int numBodies); + inline int sortConstraintByBatch2(b3Contact4* cs, int n, int simdWidth, int staticIdx, int numBodies); + inline int sortConstraintByBatch3(b3Contact4* cs, int n, int simdWidth, int staticIdx, int numBodies, int* batchSizes); - - void solveContactConstraintBatchSizes( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations, const b3AlignedObjectArray<int>* batchSizes);//const b3OpenCLArray<int>* gpuBatchSizes); + void solveContactConstraintBatchSizes(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches, int numIterations, const b3AlignedObjectArray<int>* batchSizes); //const b3OpenCLArray<int>* gpuBatchSizes); - void solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations, const b3AlignedObjectArray<int>* batchSizes);//const b3OpenCLArray<int>* gpuBatchSizes); + void solveContactConstraint(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches, int numIterations, const b3AlignedObjectArray<int>* batchSizes); //const b3OpenCLArray<int>* gpuBatchSizes); public: - - b3GpuPgsContactSolver(cl_context ctx,cl_device_id device, cl_command_queue q,int pairCapacity); + b3GpuPgsContactSolver(cl_context ctx, cl_device_id device, cl_command_queue q, int pairCapacity); virtual ~b3GpuPgsContactSolver(); void solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index); - }; -#endif //B3_GPU_BATCHING_PGS_SOLVER_H - +#endif //B3_GPU_BATCHING_PGS_SOLVER_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp index 783e443060..fef33ad1cd 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -47,7 +47,7 @@ bool gClearPairsOnGpu = true; #define TEST_OTHER_GPU_SOLVER 1 #ifdef TEST_OTHER_GPU_SOLVER #include "b3GpuJacobiContactSolver.h" -#endif //TEST_OTHER_GPU_SOLVER +#endif //TEST_OTHER_GPU_SOLVER #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" @@ -59,73 +59,68 @@ bool gClearPairsOnGpu = true; #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" #include "Bullet3OpenCL/Raycast/b3GpuRaycast.h" - #include "Bullet3Dynamics/shared/b3IntegrateTransforms.h" #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h" -b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q,class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap , struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config) +b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx, cl_device_id device, cl_command_queue q, class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config) { m_data = new b3GpuRigidBodyPipelineInternalData; - m_data->m_constraintUid=0; + m_data->m_constraintUid = 0; m_data->m_config = config; m_data->m_context = ctx; m_data->m_device = device; m_data->m_queue = q; - m_data->m_solver = new b3PgsJacobiSolver(true);//new b3PgsJacobiSolver(true); - m_data->m_gpuSolver = new b3GpuPgsConstraintSolver(ctx,device,q,true);//new b3PgsJacobiSolver(true); - - m_data->m_allAabbsGPU = new b3OpenCLArray<b3SapAabb>(ctx,q,config.m_maxConvexBodies); - m_data->m_overlappingPairsGPU = new b3OpenCLArray<b3BroadphasePair>(ctx,q,config.m_maxBroadphasePairs); + m_data->m_solver = new b3PgsJacobiSolver(true); //new b3PgsJacobiSolver(true); + m_data->m_gpuSolver = new b3GpuPgsConstraintSolver(ctx, device, q, true); //new b3PgsJacobiSolver(true); - m_data->m_gpuConstraints = new b3OpenCLArray<b3GpuGenericConstraint>(ctx,q); + m_data->m_allAabbsGPU = new b3OpenCLArray<b3SapAabb>(ctx, q, config.m_maxConvexBodies); + m_data->m_overlappingPairsGPU = new b3OpenCLArray<b3BroadphasePair>(ctx, q, config.m_maxBroadphasePairs); + + m_data->m_gpuConstraints = new b3OpenCLArray<b3GpuGenericConstraint>(ctx, q); #ifdef TEST_OTHER_GPU_SOLVER - m_data->m_solver3 = new b3GpuJacobiContactSolver(ctx,device,q,config.m_maxBroadphasePairs); -#endif // TEST_OTHER_GPU_SOLVER - - m_data->m_solver2 = new b3GpuPgsContactSolver(ctx,device,q,config.m_maxBroadphasePairs); + m_data->m_solver3 = new b3GpuJacobiContactSolver(ctx, device, q, config.m_maxBroadphasePairs); +#endif // TEST_OTHER_GPU_SOLVER + + m_data->m_solver2 = new b3GpuPgsContactSolver(ctx, device, q, config.m_maxBroadphasePairs); - m_data->m_raycaster = new b3GpuRaycast(ctx,device,q); + m_data->m_raycaster = new b3GpuRaycast(ctx, device, q); - m_data->m_broadphaseDbvt = broadphaseDbvt; m_data->m_broadphaseSap = broadphaseSap; m_data->m_narrowphase = narrowphase; - m_data->m_gravity.setValue(0.f,-9.8f,0.f); + m_data->m_gravity.setValue(0.f, -9.8f, 0.f); - cl_int errNum=0; + cl_int errNum = 0; { - cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,integrateKernelCL,&errNum,"",B3_RIGIDBODY_INTEGRATE_PATH); - b3Assert(errNum==CL_SUCCESS); - m_data->m_integrateTransformsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,integrateKernelCL, "integrateTransformsKernel",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context, m_data->m_device, integrateKernelCL, &errNum, "", B3_RIGIDBODY_INTEGRATE_PATH); + b3Assert(errNum == CL_SUCCESS); + m_data->m_integrateTransformsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device, integrateKernelCL, "integrateTransformsKernel", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); clReleaseProgram(prog); } { - cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,updateAabbsKernelCL,&errNum,"",B3_RIGIDBODY_UPDATEAABB_PATH); - b3Assert(errNum==CL_SUCCESS); - m_data->m_updateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "initializeGpuAabbsFull",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context, m_data->m_device, updateAabbsKernelCL, &errNum, "", B3_RIGIDBODY_UPDATEAABB_PATH); + b3Assert(errNum == CL_SUCCESS); + m_data->m_updateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device, updateAabbsKernelCL, "initializeGpuAabbsFull", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); - - m_data->m_clearOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "clearOverlappingPairsKernel",&errNum,prog); - b3Assert(errNum==CL_SUCCESS); + m_data->m_clearOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device, updateAabbsKernelCL, "clearOverlappingPairsKernel", &errNum, prog); + b3Assert(errNum == CL_SUCCESS); clReleaseProgram(prog); } - - } b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline() { if (m_data->m_integrateTransformsKernel) clReleaseKernel(m_data->m_integrateTransformsKernel); - + if (m_data->m_updateAabbsKernel) clReleaseKernel(m_data->m_updateAabbsKernel); - + if (m_data->m_clearOverlappingPairsKernel) clReleaseKernel(m_data->m_clearOverlappingPairsKernel); delete m_data->m_raycaster; @@ -136,15 +131,14 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline() #ifdef TEST_OTHER_GPU_SOLVER delete m_data->m_solver3; -#endif //TEST_OTHER_GPU_SOLVER - +#endif //TEST_OTHER_GPU_SOLVER + delete m_data->m_solver2; - - + delete m_data; } -void b3GpuRigidBodyPipeline::reset() +void b3GpuRigidBodyPipeline::reset() { m_data->m_gpuConstraints->resize(0); m_data->m_cpuConstraints.resize(0); @@ -152,30 +146,28 @@ void b3GpuRigidBodyPipeline::reset() m_data->m_allAabbsCPU.resize(0); } -void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint) +void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint) { m_data->m_joints.push_back(constraint); } -void b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint) +void b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint) { m_data->m_joints.remove(constraint); } - - -void b3GpuRigidBodyPipeline::removeConstraintByUid(int uid) +void b3GpuRigidBodyPipeline::removeConstraintByUid(int uid) { m_data->m_gpuSolver->recomputeBatches(); //slow linear search m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints); //remove - for (int i=0;i<m_data->m_cpuConstraints.size();i++) + for (int i = 0; i < m_data->m_cpuConstraints.size(); i++) { if (m_data->m_cpuConstraints[i].m_uid == uid) { //m_data->m_cpuConstraints.remove(m_data->m_cpuConstraints[i]); - m_data->m_cpuConstraints.swap(i,m_data->m_cpuConstraints.size()-1); + m_data->m_cpuConstraints.swap(i, m_data->m_cpuConstraints.size() - 1); m_data->m_cpuConstraints.pop_back(); break; @@ -185,13 +177,13 @@ void b3GpuRigidBodyPipeline::removeConstraintByUid(int uid) if (m_data->m_cpuConstraints.size()) { m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints); - } else + } + else { m_data->m_gpuConstraints->resize(0); } - } -int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold) +int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold) { m_data->m_gpuSolver->recomputeBatches(); b3GpuGenericConstraint c; @@ -200,14 +192,14 @@ int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, co c.m_flags = B3_CONSTRAINT_FLAG_ENABLED; c.m_rbA = bodyA; c.m_rbB = bodyB; - c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); - c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); + c.m_pivotInA.setValue(pivotInA[0], pivotInA[1], pivotInA[2]); + c.m_pivotInB.setValue(pivotInB[0], pivotInB[1], pivotInB[2]); c.m_breakingImpulseThreshold = breakingThreshold; c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE; m_data->m_cpuConstraints.push_back(c); return c.m_uid; } -int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB,float breakingThreshold) +int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold) { m_data->m_gpuSolver->recomputeBatches(); b3GpuGenericConstraint c; @@ -216,9 +208,9 @@ int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const fl c.m_flags = B3_CONSTRAINT_FLAG_ENABLED; c.m_rbA = bodyA; c.m_rbB = bodyB; - c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); - c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); - c.m_relTargetAB.setValue(relTargetAB[0],relTargetAB[1],relTargetAB[2],relTargetAB[3]); + c.m_pivotInA.setValue(pivotInA[0], pivotInA[1], pivotInA[2]); + c.m_pivotInB.setValue(pivotInB[0], pivotInB[1], pivotInB[2]); + c.m_relTargetAB.setValue(relTargetAB[0], relTargetAB[1], relTargetAB[2], relTargetAB[3]); c.m_breakingImpulseThreshold = breakingThreshold; c.m_constraintType = B3_GPU_FIXED_CONSTRAINT_TYPE; @@ -226,31 +218,28 @@ int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const fl return c.m_uid; } - -void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) +void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) { - //update worldspace AABBs from local AABB/worldtransform { B3_PROFILE("setupGpuAabbs"); setupGpuAabbsFull(); } - int numPairs =0; + int numPairs = 0; //compute overlapping pairs { - if (gUseDbvt) { { B3_PROFILE("setAabb"); m_data->m_allAabbsGPU->copyToHost(m_data->m_allAabbsCPU); - for (int i=0;i<m_data->m_allAabbsCPU.size();i++) + for (int i = 0; i < m_data->m_allAabbsCPU.size(); i++) { - b3Vector3 aabbMin=b3MakeVector3(m_data->m_allAabbsCPU[i].m_min[0],m_data->m_allAabbsCPU[i].m_min[1],m_data->m_allAabbsCPU[i].m_min[2]); - b3Vector3 aabbMax=b3MakeVector3(m_data->m_allAabbsCPU[i].m_max[0],m_data->m_allAabbsCPU[i].m_max[1],m_data->m_allAabbsCPU[i].m_max[2]); - m_data->m_broadphaseDbvt->setAabb(i,aabbMin,aabbMax,0); + b3Vector3 aabbMin = b3MakeVector3(m_data->m_allAabbsCPU[i].m_min[0], m_data->m_allAabbsCPU[i].m_min[1], m_data->m_allAabbsCPU[i].m_min[2]); + b3Vector3 aabbMax = b3MakeVector3(m_data->m_allAabbsCPU[i].m_max[0], m_data->m_allAabbsCPU[i].m_max[1], m_data->m_allAabbsCPU[i].m_max[2]); + m_data->m_broadphaseDbvt->setAabb(i, aabbMin, aabbMax, 0); } } @@ -259,13 +248,14 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) m_data->m_broadphaseDbvt->calculateOverlappingPairs(); } numPairs = m_data->m_broadphaseDbvt->getOverlappingPairCache()->getNumOverlappingPairs(); - - } else + } + else { if (gUseCalculateOverlappingPairsHost) { m_data->m_broadphaseSap->calculateOverlappingPairsHost(m_data->m_config.m_maxBroadphasePairs); - } else + } + else { m_data->m_broadphaseSap->calculateOverlappingPairs(m_data->m_config.m_maxBroadphasePairs); } @@ -274,24 +264,24 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) } //compute contact points -// printf("numPairs=%d\n",numPairs); - - int numContacts = 0; + // printf("numPairs=%d\n",numPairs); + int numContacts = 0; int numBodies = m_data->m_narrowphase->getNumRigidBodies(); if (numPairs) { - cl_mem pairs =0; - cl_mem aabbsWS =0; + cl_mem pairs = 0; + cl_mem aabbsWS = 0; if (gUseDbvt) { B3_PROFILE("m_overlappingPairsGPU->copyFromHost"); m_data->m_overlappingPairsGPU->copyFromHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray()); pairs = m_data->m_overlappingPairsGPU->getBufferCL(); aabbsWS = m_data->m_allAabbsGPU->getBufferCL(); - } else + } + else { pairs = m_data->m_broadphaseSap->getOverlappingPairBuffer(); aabbsWS = m_data->m_broadphaseSap->getAabbBufferWS(); @@ -302,31 +292,27 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) //mark the contacts for each pair as 'unused' if (numPairs) { - b3OpenCLArray<b3BroadphasePair> gpuPairs(this->m_data->m_context,m_data->m_queue); - gpuPairs.setFromOpenCLBuffer(pairs,numPairs); + b3OpenCLArray<b3BroadphasePair> gpuPairs(this->m_data->m_context, m_data->m_queue); + gpuPairs.setFromOpenCLBuffer(pairs, numPairs); if (gClearPairsOnGpu) { - - //b3AlignedObjectArray<b3BroadphasePair> hostPairs;//just for debugging //gpuPairs.copyToHost(hostPairs); - b3LauncherCL launcher(m_data->m_queue,m_data->m_clearOverlappingPairsKernel,"clearOverlappingPairsKernel"); + b3LauncherCL launcher(m_data->m_queue, m_data->m_clearOverlappingPairsKernel, "clearOverlappingPairsKernel"); launcher.setBuffer(pairs); launcher.setConst(numPairs); launcher.launch1D(numPairs); - //gpuPairs.copyToHost(hostPairs); - - - } else + } + else { b3AlignedObjectArray<b3BroadphasePair> hostPairs; gpuPairs.copyToHost(hostPairs); - for (int i=0;i<hostPairs.size();i++) + for (int i = 0; i < hostPairs.size(); i++) { hostPairs[i].z = 0xffffffff; } @@ -335,7 +321,7 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) } } - m_data->m_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies); + m_data->m_narrowphase->computeContacts(pairs, numPairs, aabbsWS, numBodies); numContacts = m_data->m_narrowphase->getNumContactsGpu(); if (gUseDbvt) @@ -347,56 +333,54 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) if (gDumpContactStats && numContacts) { m_data->m_narrowphase->getContactsGpu(); - + printf("numContacts = %d\n", numContacts); - int totalPoints = 0; + int totalPoints = 0; const b3Contact4* contacts = m_data->m_narrowphase->getContactsCPU(); - for (int i=0;i<numContacts;i++) + for (int i = 0; i < numContacts; i++) { totalPoints += contacts->getNPoints(); } - printf("totalPoints=%d\n",totalPoints); - + printf("totalPoints=%d\n", totalPoints); } } - //convert contact points to contact constraints - + //solve constraints - b3OpenCLArray<b3RigidBodyData> gpuBodies(m_data->m_context,m_data->m_queue,0,true); - gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumRigidBodies()); - b3OpenCLArray<b3InertiaData> gpuInertias(m_data->m_context,m_data->m_queue,0,true); - gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumRigidBodies()); - b3OpenCLArray<b3Contact4> gpuContacts(m_data->m_context,m_data->m_queue,0,true); - gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); + b3OpenCLArray<b3RigidBodyData> gpuBodies(m_data->m_context, m_data->m_queue, 0, true); + gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(), m_data->m_narrowphase->getNumRigidBodies()); + b3OpenCLArray<b3InertiaData> gpuInertias(m_data->m_context, m_data->m_queue, 0, true); + gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(), m_data->m_narrowphase->getNumRigidBodies()); + b3OpenCLArray<b3Contact4> gpuContacts(m_data->m_context, m_data->m_queue, 0, true); + gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(), m_data->m_narrowphase->getNumContactsGpu()); - int numJoints = m_data->m_joints.size() ? m_data->m_joints.size() : m_data->m_cpuConstraints.size(); + int numJoints = m_data->m_joints.size() ? m_data->m_joints.size() : m_data->m_cpuConstraints.size(); if (useBullet2CpuSolver && numJoints) { - - // b3AlignedObjectArray<b3Contact4> hostContacts; + // b3AlignedObjectArray<b3Contact4> hostContacts; //gpuContacts.copyToHost(hostContacts); { - bool useGpu = m_data->m_joints.size()==0; + bool useGpu = m_data->m_joints.size() == 0; -// b3Contact4* contacts = numContacts? &hostContacts[0]: 0; + // b3Contact4* contacts = numContacts? &hostContacts[0]: 0; //m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,contacts,numJoints, joints); if (useGpu) { - m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(),&gpuBodies,&gpuInertias,numJoints, m_data->m_gpuConstraints); - } else + m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(), &gpuBodies, &gpuInertias, numJoints, m_data->m_gpuConstraints); + } + else { b3AlignedObjectArray<b3RigidBodyData> hostBodies; gpuBodies.copyToHost(hostBodies); b3AlignedObjectArray<b3InertiaData> hostInertias; gpuInertias.copyToHost(hostInertias); - b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0; - m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints); + b3TypedConstraint** joints = numJoints ? &m_data->m_joints[0] : 0; + m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(), &hostBodies[0], &hostInertias[0], 0, 0, numJoints, joints); gpuBodies.copyFromHost(hostBodies); } } @@ -404,22 +388,20 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) if (numContacts) { - #ifdef TEST_OTHER_GPU_SOLVER - + if (gUseJacobi) { bool useGpu = true; if (useGpu) { - bool forceHost = false; if (forceHost) { b3AlignedObjectArray<b3RigidBodyData> hostBodies; b3AlignedObjectArray<b3InertiaData> hostInertias; b3AlignedObjectArray<b3Contact4> hostContacts; - + { B3_PROFILE("copyToHost"); gpuBodies.copyToHost(hostBodies); @@ -429,25 +411,24 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) { b3JacobiSolverInfo solverInfo; - m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),solverInfo); - - + m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(), &hostContacts[0], hostContacts.size(), solverInfo); } { B3_PROFILE("copyFromHost"); gpuBodies.copyFromHost(hostBodies); } - } else - + } + else { int static0Index = m_data->m_narrowphase->getStatic0Index(); b3JacobiSolverInfo solverInfo; //m_data->m_solver3->solveContacts( >solveGroup(&gpuBodies, &gpuInertias, &gpuContacts,solverInfo); //m_data->m_solver3->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]); - m_data->m_solver3->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),m_data->m_config, static0Index); + m_data->m_solver3->solveContacts(numBodies, gpuBodies.getBufferCL(), gpuInertias.getBufferCL(), numContacts, gpuContacts.getBufferCL(), m_data->m_config, static0Index); } - } else + } + else { b3AlignedObjectArray<b3RigidBodyData> hostBodies; gpuBodies.copyToHost(hostBodies); @@ -460,17 +441,15 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) } gpuBodies.copyFromHost(hostBodies); } - - } else -#endif //TEST_OTHER_GPU_SOLVER + } + else +#endif //TEST_OTHER_GPU_SOLVER { - int static0Index = m_data->m_narrowphase->getStatic0Index(); - m_data->m_solver2->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),m_data->m_config, static0Index); - + m_data->m_solver2->solveContacts(numBodies, gpuBodies.getBufferCL(), gpuInertias.getBufferCL(), numContacts, gpuContacts.getBufferCL(), m_data->m_config, static0Index); + //m_data->m_solver4->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(), gpuBodies.getBufferCL(), gpuInertias.getBufferCL(), numContacts, gpuContacts.getBufferCL()); - - + /*m_data->m_solver3->solveContactConstraintHost( (b3OpenCLArray<RigidBodyBase::Body>*)&gpuBodies, (b3OpenCLArray<RigidBodyBase::Inertia>*)&gpuInertias, @@ -481,11 +460,9 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) } integrate(deltaTime); - } - -void b3GpuRigidBodyPipeline::integrate(float timeStep) +void b3GpuRigidBodyPipeline::integrate(float timeStep) { //integrate int numBodies = m_data->m_narrowphase->getNumRigidBodies(); @@ -493,24 +470,25 @@ void b3GpuRigidBodyPipeline::integrate(float timeStep) if (gIntegrateOnCpu) { - if(numBodies) + if (numBodies) { - b3GpuNarrowPhaseInternalData* npData = m_data->m_narrowphase->getInternalData(); + b3GpuNarrowPhaseInternalData* npData = m_data->m_narrowphase->getInternalData(); npData->m_bodyBufferGPU->copyToHost(*npData->m_bodyBufferCPU); b3RigidBodyData_t* bodies = &npData->m_bodyBufferCPU->at(0); - for (int nodeID=0;nodeID<numBodies;nodeID++) + for (int nodeID = 0; nodeID < numBodies; nodeID++) { - integrateSingleTransform( bodies,nodeID, timeStep, angularDamp, m_data->m_gravity); + integrateSingleTransform(bodies, nodeID, timeStep, angularDamp, m_data->m_gravity); } npData->m_bodyBufferGPU->copyFromHost(*npData->m_bodyBufferCPU); } - } else + } + else { - b3LauncherCL launcher(m_data->m_queue,m_data->m_integrateTransformsKernel,"m_integrateTransformsKernel"); + b3LauncherCL launcher(m_data->m_queue, m_data->m_integrateTransformsKernel, "m_integrateTransformsKernel"); launcher.setBuffer(m_data->m_narrowphase->getBodiesGpu()); - + launcher.setConst(numBodies); launcher.setConst(timeStep); launcher.setConst(angularDamp); @@ -519,12 +497,9 @@ void b3GpuRigidBodyPipeline::integrate(float timeStep) } } - - - -void b3GpuRigidBodyPipeline::setupGpuAabbsFull() +void b3GpuRigidBodyPipeline::setupGpuAabbsFull() { - cl_int ciErrNum=0; + cl_int ciErrNum = 0; int numBodies = m_data->m_narrowphase->getNumRigidBodies(); if (!numBodies) @@ -532,34 +507,35 @@ void b3GpuRigidBodyPipeline::setupGpuAabbsFull() if (gCalcWorldSpaceAabbOnCpu) { - if (numBodies) { if (gUseDbvt) { m_data->m_allAabbsCPU.resize(numBodies); m_data->m_narrowphase->readbackAllBodiesToCpu(); - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { - b3ComputeWorldAabb( i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(),&m_data->m_allAabbsCPU[0]); + b3ComputeWorldAabb(i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(), &m_data->m_allAabbsCPU[0]); } m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); - } else + } + else { m_data->m_broadphaseSap->getAllAabbsCPU().resize(numBodies); m_data->m_narrowphase->readbackAllBodiesToCpu(); - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { - b3ComputeWorldAabb( i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(),&m_data->m_broadphaseSap->getAllAabbsCPU()[0]); + b3ComputeWorldAabb(i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(), &m_data->m_broadphaseSap->getAllAabbsCPU()[0]); } m_data->m_broadphaseSap->getAllAabbsGPU().copyFromHost(m_data->m_broadphaseSap->getAllAabbsCPU()); //m_data->m_broadphaseSap->writeAabbsToGpu(); } } - } else + } + else { //__kernel void initializeGpuAabbsFull( const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global b3AABBCL* plocalShapeAABB, __global b3AABBCL* pAABB) - b3LauncherCL launcher(m_data->m_queue,m_data->m_updateAabbsKernel,"m_updateAabbsKernel"); + b3LauncherCL launcher(m_data->m_queue, m_data->m_updateAabbsKernel, "m_updateAabbsKernel"); launcher.setConst(numBodies); cl_mem bodies = m_data->m_narrowphase->getBodiesGpu(); launcher.setBuffer(bodies); @@ -568,17 +544,18 @@ void b3GpuRigidBodyPipeline::setupGpuAabbsFull() cl_mem localAabbs = m_data->m_narrowphase->getAabbLocalSpaceBufferGpu(); launcher.setBuffer(localAabbs); - cl_mem worldAabbs =0; + cl_mem worldAabbs = 0; if (gUseDbvt) { worldAabbs = m_data->m_allAabbsGPU->getBufferCL(); - } else + } + else { worldAabbs = m_data->m_broadphaseSap->getAabbBufferWS(); } launcher.setBuffer(worldAabbs); launcher.launch1D(numBodies); - + oclCHECKERROR(ciErrNum, CL_SUCCESS); } @@ -595,78 +572,68 @@ void b3GpuRigidBodyPipeline::setupGpuAabbsFull() }; */ - - - - - } - - -cl_mem b3GpuRigidBodyPipeline::getBodyBuffer() +cl_mem b3GpuRigidBodyPipeline::getBodyBuffer() { return m_data->m_narrowphase->getBodiesGpu(); } -int b3GpuRigidBodyPipeline::getNumBodies() const +int b3GpuRigidBodyPipeline::getNumBodies() const { return m_data->m_narrowphase->getNumRigidBodies(); } -void b3GpuRigidBodyPipeline::setGravity(const float* grav) +void b3GpuRigidBodyPipeline::setGravity(const float* grav) { - m_data->m_gravity.setValue(grav[0],grav[1],grav[2]); + m_data->m_gravity.setValue(grav[0], grav[1], grav[2]); } -void b3GpuRigidBodyPipeline::copyConstraintsToHost() +void b3GpuRigidBodyPipeline::copyConstraintsToHost() { m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints); } -void b3GpuRigidBodyPipeline::writeAllInstancesToGpu() +void b3GpuRigidBodyPipeline::writeAllInstancesToGpu() { m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints); } - -int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex, bool writeInstanceToGpu) +int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex, bool writeInstanceToGpu) { - - b3Vector3 aabbMin=b3MakeVector3(0,0,0),aabbMax=b3MakeVector3(0,0,0); + b3Vector3 aabbMin = b3MakeVector3(0, 0, 0), aabbMax = b3MakeVector3(0, 0, 0); - - if (collidableIndex>=0) + if (collidableIndex >= 0) { b3SapAabb localAabb = m_data->m_narrowphase->getLocalSpaceAabb(collidableIndex); - b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); - b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); - + b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]); + b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]); + b3Scalar margin = 0.01f; b3Transform t; t.setIdentity(); - t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); - t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); - b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMin,aabbMax); - } else + t.setOrigin(b3MakeVector3(position[0], position[1], position[2])); + t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3])); + b3TransformAabb(localAabbMin, localAabbMax, margin, t, aabbMin, aabbMax); + } + else { b3Error("registerPhysicsInstance using invalid collidableIndex\n"); return -1; } - - + bool writeToGpu = false; int bodyIndex = m_data->m_narrowphase->getNumRigidBodies(); - bodyIndex = m_data->m_narrowphase->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu); + bodyIndex = m_data->m_narrowphase->registerRigidBody(collidableIndex, mass, position, orientation, &aabbMin.getX(), &aabbMax.getX(), writeToGpu); - if (bodyIndex>=0) + if (bodyIndex >= 0) { if (gUseDbvt) { - m_data->m_broadphaseDbvt->createProxy(aabbMin,aabbMax,bodyIndex,0,1,1); + m_data->m_broadphaseDbvt->createProxy(aabbMin, aabbMax, bodyIndex, 0, 1, 1); b3SapAabb aabb; - for (int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { aabb.m_min[i] = aabbMin[i]; aabb.m_max[i] = aabbMax[i]; @@ -677,14 +644,16 @@ int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po { m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); } - } else + } + else { if (mass) { - m_data->m_broadphaseSap->createProxy(aabbMin,aabbMax,bodyIndex,1,1);//m_dispatcher); - } else + m_data->m_broadphaseSap->createProxy(aabbMin, aabbMax, bodyIndex, 1, 1); //m_dispatcher); + } + else { - m_data->m_broadphaseSap->createLargeProxy(aabbMin,aabbMax,bodyIndex,1,1);//m_dispatcher); + m_data->m_broadphaseSap->createLargeProxy(aabbMin, aabbMax, bodyIndex, 1, 1); //m_dispatcher); } } } @@ -699,10 +668,10 @@ int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po return bodyIndex; } -void b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults) +void b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults) { - this->m_data->m_raycaster->castRays(rays,hitResults, - getNumBodies(),this->m_data->m_narrowphase->getBodiesCpu(), - m_data->m_narrowphase->getNumCollidablesGpu(), m_data->m_narrowphase->getCollidablesCpu(), - m_data->m_narrowphase->getInternalData(), m_data->m_broadphaseSap); + this->m_data->m_raycaster->castRays(rays, hitResults, + getNumBodies(), this->m_data->m_narrowphase->getBodiesCpu(), + m_data->m_narrowphase->getNumCollidablesGpu(), m_data->m_narrowphase->getCollidablesCpu(), + m_data->m_narrowphase->getInternalData(), m_data->m_broadphaseSap); } diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h index b4eac6841a..0e5c6fec12 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h @@ -25,50 +25,46 @@ subject to the following restrictions: class b3GpuRigidBodyPipeline { protected: - struct b3GpuRigidBodyPipelineInternalData* m_data; + struct b3GpuRigidBodyPipelineInternalData* m_data; int allocateCollidable(); public: - - - b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q , class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config); + b3GpuRigidBodyPipeline(cl_context ctx, cl_device_id device, cl_command_queue q, class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config); virtual ~b3GpuRigidBodyPipeline(); - void stepSimulation(float deltaTime); - void integrate(float timeStep); - void setupGpuAabbsFull(); + void stepSimulation(float deltaTime); + void integrate(float timeStep); + void setupGpuAabbsFull(); - int registerConvexPolyhedron(class b3ConvexUtility* convex); + int registerConvexPolyhedron(class b3ConvexUtility* convex); //int registerConvexPolyhedron(const float* vertices, int strideInBytes, int numVertices, const float* scaling); //int registerSphereShape(float radius); //int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant); - + //int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling); //int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes); - - int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData, bool writeInstanceToGpu); + int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData, bool writeInstanceToGpu); //if you passed "writeInstanceToGpu" false in the registerPhysicsInstance method (for performance) you need to call writeAllInstancesToGpu after all instances are registered - void writeAllInstancesToGpu(); - void copyConstraintsToHost(); - void setGravity(const float* grav); + void writeAllInstancesToGpu(); + void copyConstraintsToHost(); + void setGravity(const float* grav); void reset(); - - int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold); + + int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold); int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold); void removeConstraintByUid(int uid); - void addConstraint(class b3TypedConstraint* constraint); - void removeConstraint(b3TypedConstraint* constraint); - - void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults); + void addConstraint(class b3TypedConstraint* constraint); + void removeConstraint(b3TypedConstraint* constraint); - cl_mem getBodyBuffer(); + void castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults); - int getNumBodies() const; + cl_mem getBodyBuffer(); + int getNumBodies() const; }; -#endif //B3_GPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file +#endif //B3_GPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h index 5ac92f97d6..e0a26fda17 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h @@ -22,52 +22,47 @@ subject to the following restrictions: #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" - #include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" #include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" - - #include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h" #include "Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h" struct b3GpuRigidBodyPipelineInternalData { + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; - cl_context m_context; - cl_device_id m_device; - cl_command_queue m_queue; + cl_kernel m_integrateTransformsKernel; + cl_kernel m_updateAabbsKernel; + cl_kernel m_clearOverlappingPairsKernel; - cl_kernel m_integrateTransformsKernel; - cl_kernel m_updateAabbsKernel; - cl_kernel m_clearOverlappingPairsKernel; - class b3PgsJacobiSolver* m_solver; - + class b3GpuPgsConstraintSolver* m_gpuSolver; class b3GpuPgsContactSolver* m_solver2; class b3GpuJacobiContactSolver* m_solver3; class b3GpuRaycast* m_raycaster; - + class b3GpuBroadphaseInterface* m_broadphaseSap; - + struct b3DynamicBvhBroadphase* m_broadphaseDbvt; - b3OpenCLArray<b3SapAabb>* m_allAabbsGPU; - b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU; - b3OpenCLArray<b3BroadphasePair>* m_overlappingPairsGPU; + b3OpenCLArray<b3SapAabb>* m_allAabbsGPU; + b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU; + b3OpenCLArray<b3BroadphasePair>* m_overlappingPairsGPU; b3OpenCLArray<b3GpuGenericConstraint>* m_gpuConstraints; b3AlignedObjectArray<b3GpuGenericConstraint> m_cpuConstraints; b3AlignedObjectArray<b3TypedConstraint*> m_joints; - int m_constraintUid; - class b3GpuNarrowPhase* m_narrowphase; - b3Vector3 m_gravity; + int m_constraintUid; + class b3GpuNarrowPhase* m_narrowphase; + b3Vector3 m_gravity; - b3Config m_config; + b3Config m_config; }; -#endif //B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H - +#endif //B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h index f2a61801ac..db815d9b31 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h @@ -13,11 +13,9 @@ subject to the following restrictions: */ //Originally written by Erwin Coumans - #ifndef B3_GPU_SOLVER_BODY_H #define B3_GPU_SOLVER_BODY_H - #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Matrix3x3.h" @@ -27,29 +25,27 @@ subject to the following restrictions: ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision #ifdef B3_USE_SSE #define USE_SIMD 1 -#endif // - - +#endif // ///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverBody +B3_ATTRIBUTE_ALIGNED16(struct) +b3GpuSolverBody { B3_DECLARE_ALIGNED_ALLOCATOR(); -// b3Transform m_worldTransformUnused; - b3Vector3 m_deltaLinearVelocity; - b3Vector3 m_deltaAngularVelocity; - b3Vector3 m_angularFactor; - b3Vector3 m_linearFactor; - b3Vector3 m_invMass; - b3Vector3 m_pushVelocity; - b3Vector3 m_turnVelocity; - b3Vector3 m_linearVelocity; - b3Vector3 m_angularVelocity; - - union - { - void* m_originalBody; - int m_originalBodyIndex; + // b3Transform m_worldTransformUnused; + b3Vector3 m_deltaLinearVelocity; + b3Vector3 m_deltaAngularVelocity; + b3Vector3 m_angularFactor; + b3Vector3 m_linearFactor; + b3Vector3 m_invMass; + b3Vector3 m_pushVelocity; + b3Vector3 m_turnVelocity; + b3Vector3 m_linearVelocity; + b3Vector3 m_angularVelocity; + + union { + void* m_originalBody; + int m_originalBodyIndex; }; int padding[3]; @@ -65,44 +61,41 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverBody return m_worldTransform; } */ - B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + B3_FORCE_INLINE void getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const { if (m_originalBody) - velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos); else - velocity.setValue(0,0,0); + velocity.setValue(0, 0, 0); } - B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const + B3_FORCE_INLINE void getAngularVelocity(b3Vector3 & angVel) const { if (m_originalBody) - angVel =m_angularVelocity+m_deltaAngularVelocity; + angVel = m_angularVelocity + m_deltaAngularVelocity; else - angVel.setValue(0,0,0); + angVel.setValue(0, 0, 0); } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude) { if (m_originalBody) { - m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude) + B3_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, b3Scalar impulseMagnitude) { if (m_originalBody) { - m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_turnVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - - const b3Vector3& getDeltaLinearVelocity() const { return m_deltaLinearVelocity; @@ -113,20 +106,19 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverBody return m_deltaAngularVelocity; } - const b3Vector3& getPushVelocity() const + const b3Vector3& getPushVelocity() const { return m_pushVelocity; } - const b3Vector3& getTurnVelocity() const + const b3Vector3& getTurnVelocity() const { return m_turnVelocity; } - //////////////////////////////////////////////// ///some internal methods, don't use them - + b3Vector3& internalGetDeltaLinearVelocity() { return m_deltaLinearVelocity; @@ -151,7 +143,7 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverBody { m_invMass = invMass; } - + b3Vector3& internalGetPushVelocity() { return m_pushVelocity; @@ -162,67 +154,57 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverBody return m_turnVelocity; } - B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity) const { - velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + velocity = m_linearVelocity + m_deltaLinearVelocity + (m_angularVelocity + m_deltaAngularVelocity).cross(rel_pos); } - B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const + B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3 & angVel) const { - angVel = m_angularVelocity+m_deltaAngularVelocity; + angVel = m_angularVelocity + m_deltaAngularVelocity; } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude) + B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude) { //if (m_originalBody) { - m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor; + m_deltaAngularVelocity += angularComponent * (impulseMagnitude * m_angularFactor); } } - - - - void writebackVelocity() + void writebackVelocity() { //if (m_originalBody>=0) { - m_linearVelocity +=m_deltaLinearVelocity; + m_linearVelocity += m_deltaLinearVelocity; m_angularVelocity += m_deltaAngularVelocity; - + //m_originalBody->setCompanionId(-1); } } - - void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) + void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) { - (void) timeStep; + (void)timeStep; if (m_originalBody) { m_linearVelocity += m_deltaLinearVelocity; m_angularVelocity += m_deltaAngularVelocity; - + //correct the position/orientation based on push/turn recovery b3Transform newTransform; - if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0) + if (m_pushVelocity[0] != 0.f || m_pushVelocity[1] != 0 || m_pushVelocity[2] != 0 || m_turnVelocity[0] != 0.f || m_turnVelocity[1] != 0 || m_turnVelocity[2] != 0) { - // b3Quaternion orn = m_worldTransform.getRotation(); -// b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform); -// m_worldTransform = newTransform; + // b3Quaternion orn = m_worldTransform.getRotation(); + // b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform); + // m_worldTransform = newTransform; } //m_worldTransform.setRotation(orn); //m_originalBody->setCompanionId(-1); } } - - - }; -#endif //B3_SOLVER_BODY_H - - +#endif //B3_SOLVER_BODY_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h index 60d235baab..7d9eea243a 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h @@ -13,11 +13,9 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef B3_GPU_SOLVER_CONSTRAINT_H #define B3_GPU_SOLVER_CONSTRAINT_H - #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Matrix3x3.h" //#include "b3JacobianEntry.h" @@ -25,58 +23,51 @@ subject to the following restrictions: //#define NO_FRICTION_TANGENTIALS 1 - - ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -B3_ATTRIBUTE_ALIGNED16 (struct) b3GpuSolverConstraint +B3_ATTRIBUTE_ALIGNED16(struct) +b3GpuSolverConstraint { B3_DECLARE_ALIGNED_ALLOCATOR(); - b3Vector3 m_relpos1CrossNormal; - b3Vector3 m_contactNormal; + b3Vector3 m_relpos1CrossNormal; + b3Vector3 m_contactNormal; - b3Vector3 m_relpos2CrossNormal; + b3Vector3 m_relpos2CrossNormal; //b3Vector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal - b3Vector3 m_angularComponentA; - b3Vector3 m_angularComponentB; - - mutable b3Scalar m_appliedPushImpulse; - mutable b3Scalar m_appliedImpulse; + b3Vector3 m_angularComponentA; + b3Vector3 m_angularComponentB; + + mutable b3Scalar m_appliedPushImpulse; + mutable b3Scalar m_appliedImpulse; int m_padding1; int m_padding2; - b3Scalar m_friction; - b3Scalar m_jacDiagABInv; - b3Scalar m_rhs; - b3Scalar m_cfm; - - b3Scalar m_lowerLimit; - b3Scalar m_upperLimit; - b3Scalar m_rhsPenetration; - union - { - void* m_originalContactPoint; - int m_originalConstraintIndex; - b3Scalar m_unusedPadding4; + b3Scalar m_friction; + b3Scalar m_jacDiagABInv; + b3Scalar m_rhs; + b3Scalar m_cfm; + + b3Scalar m_lowerLimit; + b3Scalar m_upperLimit; + b3Scalar m_rhsPenetration; + union { + void* m_originalContactPoint; + int m_originalConstraintIndex; + b3Scalar m_unusedPadding4; }; - int m_overrideNumSolverIterations; - int m_frictionIndex; + int m_overrideNumSolverIterations; + int m_frictionIndex; int m_solverBodyIdA; int m_solverBodyIdB; - - enum b3SolverConstraintType + enum b3SolverConstraintType { B3_SOLVER_CONTACT_1D = 0, B3_SOLVER_FRICTION_1D }; }; -typedef b3AlignedObjectArray<b3GpuSolverConstraint> b3GpuConstraintArray; - - -#endif //B3_GPU_SOLVER_CONSTRAINT_H - - +typedef b3AlignedObjectArray<b3GpuSolverConstraint> b3GpuConstraintArray; +#endif //B3_GPU_SOLVER_CONSTRAINT_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.cpp index 20bf6d47c5..ccf67da1a8 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.cpp +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: */ //Originally written by Takahiro Harada - #include "b3Solver.h" ///useNewBatchingKernel is a rewritten kernel using just a single thread of the warp, for experiments @@ -38,7 +37,6 @@ bool gConvertConstraintOnCpu = false; #include "kernels/batchingKernels.h" #include "kernels/batchingKernelsNew.h" - #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" #include "Bullet3Common/b3Vector3.h" @@ -48,7 +46,7 @@ struct SolverDebugInfo int m_valInt1; int m_valInt2; int m_valInt3; - + int m_valInt4; int m_valInt5; int m_valInt6; @@ -59,11 +57,10 @@ struct SolverDebugInfo int m_valInt10; int m_valInt11; - int m_valInt12; - int m_valInt13; - int m_valInt14; - int m_valInt15; - + int m_valInt12; + int m_valInt13; + int m_valInt14; + int m_valInt15; float m_val0; float m_val1; @@ -71,9 +68,6 @@ struct SolverDebugInfo float m_val3; }; - - - class SolverDeviceInl { public: @@ -84,101 +78,89 @@ public: }; }; - - b3Solver::b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity) - : - m_context(ctx), - m_device(device), - m_queue(queue), - m_batchSizes(ctx,queue), - m_nIterations(4) + : m_context(ctx), + m_device(device), + m_queue(queue), + m_batchSizes(ctx, queue), + m_nIterations(4) { - m_sort32 = new b3RadixSort32CL(ctx,device,queue); - m_scan = new b3PrefixScanCL(ctx,device,queue,B3_SOLVER_N_CELLS); - m_search = new b3BoundSearchCL(ctx,device,queue,B3_SOLVER_N_CELLS); + m_sort32 = new b3RadixSort32CL(ctx, device, queue); + m_scan = new b3PrefixScanCL(ctx, device, queue, B3_SOLVER_N_CELLS); + m_search = new b3BoundSearchCL(ctx, device, queue, B3_SOLVER_N_CELLS); - const int sortSize = B3NEXTMULTIPLEOF( pairCapacity, 512 ); + const int sortSize = B3NEXTMULTIPLEOF(pairCapacity, 512); - m_sortDataBuffer = new b3OpenCLArray<b3SortData>(ctx,queue,sortSize); - m_contactBuffer2 = new b3OpenCLArray<b3Contact4>(ctx,queue); + m_sortDataBuffer = new b3OpenCLArray<b3SortData>(ctx, queue, sortSize); + m_contactBuffer2 = new b3OpenCLArray<b3Contact4>(ctx, queue); - m_numConstraints = new b3OpenCLArray<unsigned int>(ctx,queue,B3_SOLVER_N_CELLS ); + m_numConstraints = new b3OpenCLArray<unsigned int>(ctx, queue, B3_SOLVER_N_CELLS); m_numConstraints->resize(B3_SOLVER_N_CELLS); - m_offsets = new b3OpenCLArray<unsigned int>( ctx,queue,B3_SOLVER_N_CELLS); + m_offsets = new b3OpenCLArray<unsigned int>(ctx, queue, B3_SOLVER_N_CELLS); m_offsets->resize(B3_SOLVER_N_CELLS); const char* additionalMacros = ""; -// const char* srcFileNameForCaching=""; - - + // const char* srcFileNameForCaching=""; cl_int pErrNum; const char* batchKernelSource = batchingKernelsCL; const char* batchKernelNewSource = batchingKernelsNewCL; - + const char* solverSetupSource = solverSetupCL; const char* solverSetup2Source = solverSetup2CL; const char* solveContactSource = solveContactCL; const char* solveFrictionSource = solveFrictionCL; - - - + { - - cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveContactSource, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH); + cl_program solveContactProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solveContactSource, &pErrNum, additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH); b3Assert(solveContactProg); - - cl_program solveFrictionProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveFrictionSource, &pErrNum,additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH); + + cl_program solveFrictionProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solveFrictionSource, &pErrNum, additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH); b3Assert(solveFrictionProg); - cl_program solverSetup2Prog= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetup2Source, &pErrNum,additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH); + cl_program solverSetup2Prog = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solverSetup2Source, &pErrNum, additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH); b3Assert(solverSetup2Prog); - - cl_program solverSetupProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetupSource, &pErrNum,additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH); + cl_program solverSetupProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, solverSetupSource, &pErrNum, additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH); b3Assert(solverSetupProg); - - - m_solveFrictionKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg,additionalMacros ); + + m_solveFrictionKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg, additionalMacros); b3Assert(m_solveFrictionKernel); - m_solveContactKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg,additionalMacros ); + m_solveContactKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg, additionalMacros); b3Assert(m_solveContactKernel); - - m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg,additionalMacros ); + + m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg, additionalMacros); b3Assert(m_contactToConstraintKernel); - - m_setSortDataKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + + m_setSortDataKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_setSortDataKernel); - - m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + + m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_reorderContactKernel); - - m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog,additionalMacros ); + m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog, additionalMacros); b3Assert(m_copyConstraintKernel); - } { - cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelSource, &pErrNum,additionalMacros, B3_BATCHING_PATH); + cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, batchKernelSource, &pErrNum, additionalMacros, B3_BATCHING_PATH); //cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, 0, &pErrNum,additionalMacros, B3_BATCHING_PATH,true); b3Assert(batchingProg); - - m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg,additionalMacros ); + + m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString(ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg, additionalMacros); b3Assert(m_batchingKernel); } { - cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelNewSource, &pErrNum,additionalMacros, B3_BATCHING_NEW_PATH); + cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString(ctx, device, batchKernelNewSource, &pErrNum, additionalMacros, B3_BATCHING_NEW_PATH); b3Assert(batchingNewProg); - m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg,additionalMacros ); + m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString(ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg, additionalMacros); //m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesBruteForce", &pErrNum, batchingNewProg,additionalMacros ); b3Assert(m_batchingKernelNew); } } - + b3Solver::~b3Solver() { delete m_offsets; @@ -190,71 +172,68 @@ b3Solver::~b3Solver() delete m_scan; delete m_search; - clReleaseKernel(m_batchingKernel); clReleaseKernel(m_batchingKernelNew); - - clReleaseKernel( m_solveContactKernel); - clReleaseKernel( m_solveFrictionKernel); - - clReleaseKernel( m_contactToConstraintKernel); - clReleaseKernel( m_setSortDataKernel); - clReleaseKernel( m_reorderContactKernel); - clReleaseKernel( m_copyConstraintKernel); - -} + clReleaseKernel(m_solveContactKernel); + clReleaseKernel(m_solveFrictionKernel); - + clReleaseKernel(m_contactToConstraintKernel); + clReleaseKernel(m_setSortDataKernel); + clReleaseKernel(m_reorderContactKernel); + clReleaseKernel(m_copyConstraintKernel); +} -template<bool JACOBI> -static -__inline -void solveContact(b3GpuConstraint4& cs, - const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4]) +template <bool JACOBI> +static __inline void solveContact(b3GpuConstraint4& cs, + const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4]) { - - b3Vector3 dLinVelA; dLinVelA.setZero(); - b3Vector3 dAngVelA; dAngVelA.setZero(); - b3Vector3 dLinVelB; dLinVelB.setZero(); - b3Vector3 dAngVelB; dAngVelB.setZero(); - - for(int ic=0; ic<4; ic++) + b3Vector3 dLinVelA; + dLinVelA.setZero(); + b3Vector3 dAngVelA; + dAngVelA.setZero(); + b3Vector3 dLinVelB; + dLinVelB.setZero(); + b3Vector3 dAngVelB; + dAngVelB.setZero(); + + for (int ic = 0; ic < 4; ic++) { // dont necessary because this makes change to 0 - if( cs.m_jacCoeffInv[ic] == 0.f ) continue; + if (cs.m_jacCoeffInv[ic] == 0.f) continue; { b3Vector3 angular0, angular1, linear; b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA; b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB; - setLinearAndAngular( (const b3Vector3 &)cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, &linear, &angular0, &angular1 ); + setLinearAndAngular((const b3Vector3&)cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, &linear, &angular0, &angular1); - float rambdaDt = calcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic]; + float rambdaDt = calcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB) + + cs.m_b[ic]; rambdaDt *= cs.m_jacCoeffInv[ic]; { float prevSum = cs.m_appliedRambdaDt[ic]; float updated = prevSum; updated += rambdaDt; - updated = b3Max( updated, minRambdaDt[ic] ); - updated = b3Min( updated, maxRambdaDt[ic] ); + updated = b3Max(updated, minRambdaDt[ic]); + updated = b3Min(updated, maxRambdaDt[ic]); rambdaDt = updated - prevSum; cs.m_appliedRambdaDt[ic] = updated; } - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; + b3Vector3 linImp0 = invMassA * linear * rambdaDt; + b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt; + b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt; + b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt; #ifdef _WIN32 - b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp0.getX())); b3Assert(_finite(linImp1.getX())); #endif - if( JACOBI ) + if (JACOBI) { dLinVelA += linImp0; dAngVelA += angImp0; @@ -271,92 +250,83 @@ void solveContact(b3GpuConstraint4& cs, } } - if( JACOBI ) + if (JACOBI) { linVelA += dLinVelA; angVelA += dAngVelA; linVelB += dLinVelB; angVelB += dAngVelB; } - } +static __inline void solveFriction(b3GpuConstraint4& cs, + const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, + const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, + float maxRambdaDt[4], float minRambdaDt[4]) +{ + if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return; + const b3Vector3& center = (const b3Vector3&)cs.m_center; + b3Vector3 n = -(const b3Vector3&)cs.m_linear; - - - static - __inline - void solveFriction(b3GpuConstraint4& cs, - const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, - const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, - float maxRambdaDt[4], float minRambdaDt[4]) - { - - if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; - const b3Vector3& center = (const b3Vector3&)cs.m_center; - - b3Vector3 n = -(const b3Vector3&)cs.m_linear; - - b3Vector3 tangent[2]; -#if 1 - b3PlaneSpace1 (n, tangent[0],tangent[1]); + b3Vector3 tangent[2]; +#if 1 + b3PlaneSpace1(n, tangent[0], tangent[1]); #else - b3Vector3 r = cs.m_worldPos[0]-center; - tangent[0] = cross3( n, r ); - tangent[1] = cross3( tangent[0], n ); - tangent[0] = normalize3( tangent[0] ); - tangent[1] = normalize3( tangent[1] ); + b3Vector3 r = cs.m_worldPos[0] - center; + tangent[0] = cross3(n, r); + tangent[1] = cross3(tangent[0], n); + tangent[0] = normalize3(tangent[0]); + tangent[1] = normalize3(tangent[1]); #endif - b3Vector3 angular0, angular1, linear; - b3Vector3 r0 = center - posA; - b3Vector3 r1 = center - posB; - for(int i=0; i<2; i++) - { - setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 ); - float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, - linVelA, angVelA, linVelB, angVelB ); - rambdaDt *= cs.m_fJacCoeffInv[i]; + b3Vector3 angular0, angular1, linear; + b3Vector3 r0 = center - posA; + b3Vector3 r1 = center - posB; + for (int i = 0; i < 2; i++) + { + setLinearAndAngular(tangent[i], r0, r1, &linear, &angular0, &angular1); + float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB); + rambdaDt *= cs.m_fJacCoeffInv[i]; - { - float prevSum = cs.m_fAppliedRambdaDt[i]; - float updated = prevSum; - updated += rambdaDt; - updated = b3Max( updated, minRambdaDt[i] ); - updated = b3Min( updated, maxRambdaDt[i] ); - rambdaDt = updated - prevSum; - cs.m_fAppliedRambdaDt[i] = updated; - } + { + float prevSum = cs.m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = b3Max(updated, minRambdaDt[i]); + updated = b3Min(updated, maxRambdaDt[i]); + rambdaDt = updated - prevSum; + cs.m_fAppliedRambdaDt[i] = updated; + } - b3Vector3 linImp0 = invMassA*linear*rambdaDt; - b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; - b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; - b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; + b3Vector3 linImp0 = invMassA * linear * rambdaDt; + b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt; + b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt; + b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt; #ifdef _WIN32 - b3Assert(_finite(linImp0.getX())); - b3Assert(_finite(linImp1.getX())); + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); #endif - linVelA += linImp0; - angVelA += angImp0; - linVelB += linImp1; - angVelB += angImp1; - } + linVelA += linImp0; + angVelA += angImp0; + linVelB += linImp1; + angVelB += angImp1; + } - { // angular damping for point constraint - b3Vector3 ab = ( posB - posA ).normalized(); - b3Vector3 ac = ( center - posA ).normalized(); - if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) - { - float angNA = b3Dot( n, angVelA ); - float angNB = b3Dot( n, angVelB ); + { // angular damping for point constraint + b3Vector3 ab = (posB - posA).normalized(); + b3Vector3 ac = (center - posA).normalized(); + if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { + float angNA = b3Dot(n, angVelA); + float angNB = b3Dot(n, angVelB); - angVelA -= (angNA*0.1f)*n; - angVelB -= (angNB*0.1f)*n; - } + angVelA -= (angNA * 0.1f) * n; + angVelB -= (angNB * 0.1f) * n; } - } +} /* b3AlignedObjectArray<b3RigidBodyData>& m_bodies; b3AlignedObjectArray<b3InertiaData>& m_shapes; @@ -370,79 +340,69 @@ void solveContact(b3GpuConstraint4& cs, int m_maxNumBatches; */ -struct SolveTask// : public ThreadPool::Task +struct SolveTask // : public ThreadPool::Task { - SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies, b3AlignedObjectArray<b3InertiaData>& shapes, b3AlignedObjectArray<b3GpuConstraint4>& constraints, - int start, int nConstraints,int maxNumBatches,b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx, b3AlignedObjectArray<int>* batchSizes, int cellIndex) - : m_bodies( bodies ), m_shapes( shapes ), - m_constraints( constraints ), - m_batchSizes(batchSizes), - m_cellIndex(cellIndex), - m_curWgidx(curWgidx), - m_start( start ), - m_nConstraints( nConstraints ), - m_solveFriction( true ), - m_maxNumBatches(maxNumBatches) - {} - - unsigned short int getType(){ return 0; } + SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies, b3AlignedObjectArray<b3InertiaData>& shapes, b3AlignedObjectArray<b3GpuConstraint4>& constraints, + int start, int nConstraints, int maxNumBatches, b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx, b3AlignedObjectArray<int>* batchSizes, int cellIndex) + : m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_batchSizes(batchSizes), m_cellIndex(cellIndex), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches) + { + } + + unsigned short int getType() { return 0; } void run(int tIdx) { int offset = 0; - for (int ii=0;ii<B3_MAX_NUM_BATCHES;ii++) + for (int ii = 0; ii < B3_MAX_NUM_BATCHES; ii++) { - int numInBatch = m_batchSizes->at(m_cellIndex*B3_MAX_NUM_BATCHES+ii); + int numInBatch = m_batchSizes->at(m_cellIndex * B3_MAX_NUM_BATCHES + ii); if (!numInBatch) break; - for (int jj=0;jj<numInBatch;jj++) + for (int jj = 0; jj < numInBatch; jj++) { - int i = m_start + offset+jj; + int i = m_start + offset + jj; int batchId = m_constraints[i].m_batchIdx; - b3Assert(batchId==ii); + b3Assert(batchId == ii); float frictionCoeff = m_constraints[i].getFrictionCoeff(); int aIdx = (int)m_constraints[i].m_bodyA; int bIdx = (int)m_constraints[i].m_bodyB; -// int localBatch = m_constraints[i].m_batchIdx; + // int localBatch = m_constraints[i].m_batchIdx; b3RigidBodyData& bodyA = m_bodies[aIdx]; b3RigidBodyData& bodyB = m_bodies[bIdx]; - if( !m_solveFriction ) + if (!m_solveFriction) { - float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; - float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; + float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX}; + float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f}; - solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, - (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt ); + solveContact<false>(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt); } else { - float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; - float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; + float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX}; + float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f}; float sum = 0; - for(int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { - sum +=m_constraints[i].m_appliedRambdaDt[j]; + sum += m_constraints[i].m_appliedRambdaDt[j]; } frictionCoeff = 0.7f; - for(int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { - maxRambdaDt[j] = frictionCoeff*sum; + maxRambdaDt[j] = frictionCoeff * sum; minRambdaDt[j] = -maxRambdaDt[j]; } - solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, - (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld, - maxRambdaDt, minRambdaDt ); - + solveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt); } } - offset+=numInBatch; - - + offset += numInBatch; } -/* for (int bb=0;bb<m_maxNumBatches;bb++) + /* for (int bb=0;bb<m_maxNumBatches;bb++) { //for(int ic=m_nConstraints-1; ic>=0; ic--) for(int ic=0; ic<m_nConstraints; ic++) @@ -491,9 +451,6 @@ struct SolveTask// : public ThreadPool::Task } } */ - - - } b3AlignedObjectArray<b3RigidBodyData>& m_bodies; @@ -508,11 +465,9 @@ struct SolveTask// : public ThreadPool::Task int m_maxNumBatches; }; - -void b3Solver::solveContactConstraintHost( b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches,b3AlignedObjectArray<int>* batchSizes) +void b3Solver::solveContactConstraintHost(b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches, b3AlignedObjectArray<int>* batchSizes) { - #if 0 { int nSplitX = B3_SOLVER_N_SPLIT_X; @@ -571,114 +526,105 @@ void b3Solver::solveContactConstraintHost( b3OpenCLArray<b3RigidBodyData>* body //printf("------------------------\n"); b3AlignedObjectArray<unsigned int> offsetsHost; m_offsets->copyToHost(offsetsHost); - static int frame=0; - bool useBatches=true; + static int frame = 0; + bool useBatches = true; if (useBatches) { - for(int iter=0; iter<m_nIterations; iter++) + for (int iter = 0; iter < m_nIterations; iter++) { - for (int cellBatch=0;cellBatch<B3_SOLVER_N_BATCHES;cellBatch++) + for (int cellBatch = 0; cellBatch < B3_SOLVER_N_BATCHES; cellBatch++) { - int nSplitX = B3_SOLVER_N_SPLIT_X; int nSplitY = B3_SOLVER_N_SPLIT_Y; - int numWorkgroups = B3_SOLVER_N_CELLS/B3_SOLVER_N_BATCHES; + int numWorkgroups = B3_SOLVER_N_CELLS / B3_SOLVER_N_BATCHES; //printf("cell Batch %d\n",cellBatch); b3AlignedObjectArray<int> usedBodies[B3_SOLVER_N_CELLS]; - for (int i=0;i<B3_SOLVER_N_CELLS;i++) + for (int i = 0; i < B3_SOLVER_N_CELLS; i++) { usedBodies[i].resize(0); } - - - //for (int wgIdx=numWorkgroups-1;wgIdx>=0;wgIdx--) - for (int wgIdx=0;wgIdx<numWorkgroups;wgIdx++) + for (int wgIdx = 0; wgIdx < numWorkgroups; wgIdx++) { - int zIdx = (wgIdx/((nSplitX*nSplitY)/4))*2+((cellBatch&4)>>2); - int remain= (wgIdx%((nSplitX*nSplitY)/4)); - int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1); - int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1); - int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY); - - - if( numConstraintsHost[cellIdx] == 0 ) + int zIdx = (wgIdx / ((nSplitX * nSplitY) / 4)) * 2 + ((cellBatch & 4) >> 2); + int remain = (wgIdx % ((nSplitX * nSplitY) / 4)); + int yIdx = (remain / (nSplitX / 2)) * 2 + ((cellBatch & 2) >> 1); + int xIdx = (remain % (nSplitX / 2)) * 2 + (cellBatch & 1); + int cellIdx = xIdx + yIdx * nSplitX + zIdx * (nSplitX * nSplitY); + + if (numConstraintsHost[cellIdx] == 0) continue; //printf("wgIdx %d: xIdx=%d, yIdx=%d, zIdx=%d, cellIdx=%d, cell Batch %d\n",wgIdx,xIdx,yIdx,zIdx,cellIdx,cellBatch); //printf("cell %d has %d constraints\n", cellIdx,numConstraintsHost[cellIdx]); if (zIdx) { - //printf("?\n"); + //printf("?\n"); } - if (iter==0) + if (iter == 0) { //printf("frame=%d, Cell xIdx=%x, yIdx=%d ",frame, xIdx,yIdx); //printf("cellBatch=%d, wgIdx=%d, #constraints in cell=%d\n",cellBatch,wgIdx,numConstraintsHost[cellIdx]); } const int start = offsetsHost[cellIdx]; int numConstraintsInCell = numConstraintsHost[cellIdx]; - // const int end = start + numConstraintsInCell; + // const int end = start + numConstraintsInCell; - SolveTask task( bodyNative, shapeNative, constraintNative, start, numConstraintsInCell ,maxNumBatches,usedBodies,wgIdx,batchSizes,cellIdx); + SolveTask task(bodyNative, shapeNative, constraintNative, start, numConstraintsInCell, maxNumBatches, usedBodies, wgIdx, batchSizes, cellIdx); task.m_solveFriction = false; task.run(0); - } } } - for(int iter=0; iter<m_nIterations; iter++) + for (int iter = 0; iter < m_nIterations; iter++) { - for (int cellBatch=0;cellBatch<B3_SOLVER_N_BATCHES;cellBatch++) + for (int cellBatch = 0; cellBatch < B3_SOLVER_N_BATCHES; cellBatch++) { int nSplitX = B3_SOLVER_N_SPLIT_X; int nSplitY = B3_SOLVER_N_SPLIT_Y; - - int numWorkgroups = B3_SOLVER_N_CELLS/B3_SOLVER_N_BATCHES; + int numWorkgroups = B3_SOLVER_N_CELLS / B3_SOLVER_N_BATCHES; - for (int wgIdx=0;wgIdx<numWorkgroups;wgIdx++) + for (int wgIdx = 0; wgIdx < numWorkgroups; wgIdx++) { - int zIdx = (wgIdx/((nSplitX*nSplitY)/4))*2+((cellBatch&4)>>2); - int remain= (wgIdx%((nSplitX*nSplitY)/4)); - int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1); - int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1); - - int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY); - - if( numConstraintsHost[cellIdx] == 0 ) + int zIdx = (wgIdx / ((nSplitX * nSplitY) / 4)) * 2 + ((cellBatch & 4) >> 2); + int remain = (wgIdx % ((nSplitX * nSplitY) / 4)); + int yIdx = (remain / (nSplitX / 2)) * 2 + ((cellBatch & 2) >> 1); + int xIdx = (remain % (nSplitX / 2)) * 2 + (cellBatch & 1); + + int cellIdx = xIdx + yIdx * nSplitX + zIdx * (nSplitX * nSplitY); + + if (numConstraintsHost[cellIdx] == 0) continue; - + //printf("yIdx=%d\n",yIdx); - + const int start = offsetsHost[cellIdx]; int numConstraintsInCell = numConstraintsHost[cellIdx]; - // const int end = start + numConstraintsInCell; + // const int end = start + numConstraintsInCell; - SolveTask task( bodyNative, shapeNative, constraintNative, start, numConstraintsInCell,maxNumBatches, 0,0,batchSizes,cellIdx); + SolveTask task(bodyNative, shapeNative, constraintNative, start, numConstraintsInCell, maxNumBatches, 0, 0, batchSizes, cellIdx); task.m_solveFriction = true; task.run(0); - } } } - - - } else + } + else { - for(int iter=0; iter<m_nIterations; iter++) + for (int iter = 0; iter < m_nIterations; iter++) { - SolveTask task( bodyNative, shapeNative, constraintNative, 0, n ,maxNumBatches,0,0,0,0); + SolveTask task(bodyNative, shapeNative, constraintNative, 0, n, maxNumBatches, 0, 0, 0, 0); task.m_solveFriction = false; task.run(0); } - for(int iter=0; iter<m_nIterations; iter++) + for (int iter = 0; iter < m_nIterations; iter++) { - SolveTask task( bodyNative, shapeNative, constraintNative, 0, n ,maxNumBatches,0,0,0,0); + SolveTask task(bodyNative, shapeNative, constraintNative, 0, n, maxNumBatches, 0, 0, 0, 0); task.m_solveFriction = true; task.run(0); } @@ -688,23 +634,21 @@ void b3Solver::solveContactConstraintHost( b3OpenCLArray<b3RigidBodyData>* body shapeBuf->copyFromHost(shapeNative); constraint->copyFromHost(constraintNative); frame++; - } void checkConstraintBatch(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, - const b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, - b3OpenCLArray<unsigned int>* m_numConstraints, - b3OpenCLArray<unsigned int>* m_offsets, - int batchId - ) + const b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, + b3OpenCLArray<unsigned int>* m_numConstraints, + b3OpenCLArray<unsigned int>* m_offsets, + int batchId) { -// b3BufferInfoCL( m_numConstraints->getBufferCL() ), -// b3BufferInfoCL( m_offsets->getBufferCL() ) - + // b3BufferInfoCL( m_numConstraints->getBufferCL() ), + // b3BufferInfoCL( m_offsets->getBufferCL() ) + int cellBatch = batchId; const int nn = B3_SOLVER_N_CELLS; -// int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; + // int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; b3AlignedObjectArray<unsigned int> gN; m_numConstraints->copyToHost(gN); @@ -712,243 +656,220 @@ void checkConstraintBatch(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, m_offsets->copyToHost(gOffsets); int nSplitX = B3_SOLVER_N_SPLIT_X; int nSplitY = B3_SOLVER_N_SPLIT_Y; - -// int bIdx = batchId; + + // int bIdx = batchId; b3AlignedObjectArray<b3GpuConstraint4> cpuConstraints; constraint->copyToHost(cpuConstraints); printf("batch = %d\n", batchId); - int numWorkgroups = nn/B3_SOLVER_N_BATCHES; + int numWorkgroups = nn / B3_SOLVER_N_BATCHES; b3AlignedObjectArray<int> usedBodies; - - for (int wgIdx=0;wgIdx<numWorkgroups;wgIdx++) + for (int wgIdx = 0; wgIdx < numWorkgroups; wgIdx++) { printf("wgIdx = %d ", wgIdx); - int zIdx = (wgIdx/((nSplitX*nSplitY))/2)*2+((cellBatch&4)>>2); - int remain = wgIdx%((nSplitX*nSplitY)); - int yIdx = (remain%(nSplitX/2))*2 + ((cellBatch&2)>>1); - int xIdx = (remain/(nSplitX/2))*2 + (cellBatch&1); + int zIdx = (wgIdx / ((nSplitX * nSplitY)) / 2) * 2 + ((cellBatch & 4) >> 2); + int remain = wgIdx % ((nSplitX * nSplitY)); + int yIdx = (remain % (nSplitX / 2)) * 2 + ((cellBatch & 2) >> 1); + int xIdx = (remain / (nSplitX / 2)) * 2 + (cellBatch & 1); - - int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY); - printf("cellIdx=%d\n",cellIdx); - if( gN[cellIdx] == 0 ) + int cellIdx = xIdx + yIdx * nSplitX + zIdx * (nSplitX * nSplitY); + printf("cellIdx=%d\n", cellIdx); + if (gN[cellIdx] == 0) continue; const int start = gOffsets[cellIdx]; const int end = start + gN[cellIdx]; - for (int c=start;c<end;c++) + for (int c = start; c < end; c++) { b3GpuConstraint4& constraint = cpuConstraints[c]; //printf("constraint (%d,%d)\n", constraint.m_bodyA,constraint.m_bodyB); - if (usedBodies.findLinearSearch(constraint.m_bodyA)< usedBodies.size()) + if (usedBodies.findLinearSearch(constraint.m_bodyA) < usedBodies.size()) { printf("error?\n"); } - if (usedBodies.findLinearSearch(constraint.m_bodyB)< usedBodies.size()) + if (usedBodies.findLinearSearch(constraint.m_bodyB) < usedBodies.size()) { printf("error?\n"); } } - for (int c=start;c<end;c++) + for (int c = start; c < end; c++) { b3GpuConstraint4& constraint = cpuConstraints[c]; usedBodies.push_back(constraint.m_bodyA); usedBodies.push_back(constraint.m_bodyB); } - } } -static bool verify=false; +static bool verify = false; -void b3Solver::solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches) +void b3Solver::solveContactConstraint(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches) { - - - b3Int4 cdata = b3MakeInt4( n, 0, 0, 0 ); + b3Int4 cdata = b3MakeInt4(n, 0, 0, 0); { - const int nn = B3_SOLVER_N_CELLS; cdata.x = 0; - cdata.y = maxNumBatches;//250; - + cdata.y = maxNumBatches; //250; - int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; + int numWorkItems = 64 * nn / B3_SOLVER_N_BATCHES; #ifdef DEBUG_ME - SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; - adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device,numWorkItems); + SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; + adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device, numWorkItems); #endif - - { - B3_PROFILE("m_batchSolveKernel iterations"); - for(int iter=0; iter<m_nIterations; iter++) + for (int iter = 0; iter < m_nIterations; iter++) { - for(int ib=0; ib<B3_SOLVER_N_BATCHES; ib++) + for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++) { - if (verify) { - checkConstraintBatch(bodyBuf,shapeBuf,constraint,m_numConstraints,m_offsets,ib); + checkConstraintBatch(bodyBuf, shapeBuf, constraint, m_numConstraints, m_offsets, ib); } #ifdef DEBUG_ME - memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems); - gpuDebugInfo.write(debugInfo,numWorkItems); + memset(debugInfo, 0, sizeof(SolverDebugInfo) * numWorkItems); + gpuDebugInfo.write(debugInfo, numWorkItems); #endif - cdata.z = ib; - - b3LauncherCL launcher( m_queue, m_solveContactKernel ,"m_solveContactKernel"); + b3LauncherCL launcher(m_queue, m_solveContactKernel, "m_solveContactKernel"); #if 1 - - b3BufferInfoCL bInfo[] = { - - b3BufferInfoCL( bodyBuf->getBufferCL() ), - b3BufferInfoCL( shapeBuf->getBufferCL() ), - b3BufferInfoCL( constraint->getBufferCL() ), - b3BufferInfoCL( m_numConstraints->getBufferCL() ), - b3BufferInfoCL( m_offsets->getBufferCL() ) + + b3BufferInfoCL bInfo[] = { + + b3BufferInfoCL(bodyBuf->getBufferCL()), + b3BufferInfoCL(shapeBuf->getBufferCL()), + b3BufferInfoCL(constraint->getBufferCL()), + b3BufferInfoCL(m_numConstraints->getBufferCL()), + b3BufferInfoCL(m_offsets->getBufferCL()) #ifdef DEBUG_ME - , b3BufferInfoCL(&gpuDebugInfo) + , + b3BufferInfoCL(&gpuDebugInfo) #endif - }; - - + }; - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); //launcher.setConst( cdata.x ); - launcher.setConst( cdata.y ); - launcher.setConst( cdata.z ); - b3Int4 nSplit; + launcher.setConst(cdata.y); + launcher.setConst(cdata.z); + b3Int4 nSplit; nSplit.x = B3_SOLVER_N_SPLIT_X; nSplit.y = B3_SOLVER_N_SPLIT_Y; nSplit.z = B3_SOLVER_N_SPLIT_Z; - launcher.setConst( nSplit ); - launcher.launch1D( numWorkItems, 64 ); + launcher.setConst(nSplit); + launcher.launch1D(numWorkItems, 64); - #else - const char* fileName = "m_batchSolveKernel.bin"; - FILE* f = fopen(fileName,"rb"); - if (f) - { - int sizeInBytes=0; - if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET)) - { - printf("error, cannot get file size\n"); - exit(0); - } - - unsigned char* buf = (unsigned char*) malloc(sizeInBytes); - fread(buf,sizeInBytes,1,f); - int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes,m_context); - int num = *(int*)&buf[serializedBytes]; - - launcher.launch1D( num); - - //this clFinish is for testing on errors - clFinish(m_queue); - } + const char* fileName = "m_batchSolveKernel.bin"; + FILE* f = fopen(fileName, "rb"); + if (f) + { + int sizeInBytes = 0; + if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET)) + { + printf("error, cannot get file size\n"); + exit(0); + } + + unsigned char* buf = (unsigned char*)malloc(sizeInBytes); + fread(buf, sizeInBytes, 1, f); + int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes, m_context); + int num = *(int*)&buf[serializedBytes]; + + launcher.launch1D(num); + + //this clFinish is for testing on errors + clFinish(m_queue); + } #endif - #ifdef DEBUG_ME clFinish(m_queue); - gpuDebugInfo.read(debugInfo,numWorkItems); + gpuDebugInfo.read(debugInfo, numWorkItems); clFinish(m_queue); - for (int i=0;i<numWorkItems;i++) + for (int i = 0; i < numWorkItems; i++) { - if (debugInfo[i].m_valInt2>0) + if (debugInfo[i].m_valInt2 > 0) { - printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2); + printf("debugInfo[i].m_valInt2 = %d\n", i, debugInfo[i].m_valInt2); } - if (debugInfo[i].m_valInt3>0) + if (debugInfo[i].m_valInt3 > 0) { - printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3); + printf("debugInfo[i].m_valInt3 = %d\n", i, debugInfo[i].m_valInt3); } } -#endif //DEBUG_ME - - +#endif //DEBUG_ME } } - - clFinish(m_queue); - + clFinish(m_queue); } cdata.x = 1; - bool applyFriction=true; + bool applyFriction = true; if (applyFriction) - { + { B3_PROFILE("m_batchSolveKernel iterations2"); - for(int iter=0; iter<m_nIterations; iter++) + for (int iter = 0; iter < m_nIterations; iter++) { - for(int ib=0; ib<B3_SOLVER_N_BATCHES; ib++) + for (int ib = 0; ib < B3_SOLVER_N_BATCHES; ib++) { cdata.z = ib; - - - b3BufferInfoCL bInfo[] = { - b3BufferInfoCL( bodyBuf->getBufferCL() ), - b3BufferInfoCL( shapeBuf->getBufferCL() ), - b3BufferInfoCL( constraint->getBufferCL() ), - b3BufferInfoCL( m_numConstraints->getBufferCL() ), - b3BufferInfoCL( m_offsets->getBufferCL() ) + + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL(bodyBuf->getBufferCL()), + b3BufferInfoCL(shapeBuf->getBufferCL()), + b3BufferInfoCL(constraint->getBufferCL()), + b3BufferInfoCL(m_numConstraints->getBufferCL()), + b3BufferInfoCL(m_offsets->getBufferCL()) #ifdef DEBUG_ME - ,b3BufferInfoCL(&gpuDebugInfo) -#endif //DEBUG_ME + , + b3BufferInfoCL(&gpuDebugInfo) +#endif //DEBUG_ME }; - b3LauncherCL launcher( m_queue, m_solveFrictionKernel,"m_solveFrictionKernel" ); - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + b3LauncherCL launcher(m_queue, m_solveFrictionKernel, "m_solveFrictionKernel"); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); //launcher.setConst( cdata.x ); - launcher.setConst( cdata.y ); - launcher.setConst( cdata.z ); - b3Int4 nSplit; + launcher.setConst(cdata.y); + launcher.setConst(cdata.z); + b3Int4 nSplit; nSplit.x = B3_SOLVER_N_SPLIT_X; nSplit.y = B3_SOLVER_N_SPLIT_Y; nSplit.z = B3_SOLVER_N_SPLIT_Z; - launcher.setConst( nSplit ); - - launcher.launch1D( 64*nn/B3_SOLVER_N_BATCHES, 64 ); + launcher.setConst(nSplit); + + launcher.launch1D(64 * nn / B3_SOLVER_N_BATCHES, 64); } } clFinish(m_queue); - } #ifdef DEBUG_ME delete[] debugInfo; -#endif //DEBUG_ME +#endif //DEBUG_ME } - - } -void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, - const b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData, - int nContacts, const ConstraintCfg& cfg ) +void b3Solver::convertToConstraints(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, + const b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData, + int nContacts, const ConstraintCfg& cfg) { -// b3OpenCLArray<b3GpuConstraint4>* constraintNative =0; + // b3OpenCLArray<b3GpuConstraint4>* constraintNative =0; contactCOut->resize(nContacts); struct CB { @@ -959,30 +880,28 @@ void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyData>* bodyB }; { - CB cdata; cdata.m_nContacts = nContacts; cdata.m_dt = cfg.m_dt; cdata.m_positionDrift = cfg.m_positionDrift; cdata.m_positionConstraintCoeff = cfg.m_positionConstraintCoeff; - if (gConvertConstraintOnCpu) { b3AlignedObjectArray<b3RigidBodyData> gBodies; - bodyBuf->copyToHost(gBodies); + bodyBuf->copyToHost(gBodies); - b3AlignedObjectArray<b3Contact4> gContact; - contactsIn->copyToHost(gContact); + b3AlignedObjectArray<b3Contact4> gContact; + contactsIn->copyToHost(gContact); + + b3AlignedObjectArray<b3InertiaData> gShapes; + shapeBuf->copyToHost(gShapes); + + b3AlignedObjectArray<b3GpuConstraint4> gConstraintOut; + gConstraintOut.resize(nContacts); - b3AlignedObjectArray<b3InertiaData> gShapes; - shapeBuf->copyToHost(gShapes); - - b3AlignedObjectArray<b3GpuConstraint4> gConstraintOut; - gConstraintOut.resize(nContacts); - B3_PROFILE("cpu contactToConstraintKernel"); - for (int gIdx=0;gIdx<nContacts;gIdx++) + for (int gIdx = 0; gIdx < nContacts; gIdx++) { int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); @@ -1001,40 +920,36 @@ void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyData>* bodyB b3ContactConstraint4_t cs; - setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, - &gContact[gIdx], cdata.m_dt, cdata.m_positionDrift, cdata.m_positionConstraintCoeff, - &cs ); - + setConstraint4(posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, + &gContact[gIdx], cdata.m_dt, cdata.m_positionDrift, cdata.m_positionConstraintCoeff, + &cs); + cs.m_batchIdx = gContact[gIdx].m_batchIdx; gConstraintOut[gIdx] = (b3GpuConstraint4&)cs; } contactCOut->copyFromHost(gConstraintOut); - - } else + } + else { B3_PROFILE("gpu m_contactToConstraintKernel"); - - b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( shapeBuf->getBufferCL()), - b3BufferInfoCL( contactCOut->getBufferCL() )}; - b3LauncherCL launcher( m_queue, m_contactToConstraintKernel,"m_contactToConstraintKernel" ); - launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + b3BufferInfoCL bInfo[] = {b3BufferInfoCL(contactsIn->getBufferCL()), b3BufferInfoCL(bodyBuf->getBufferCL()), b3BufferInfoCL(shapeBuf->getBufferCL()), + b3BufferInfoCL(contactCOut->getBufferCL())}; + b3LauncherCL launcher(m_queue, m_contactToConstraintKernel, "m_contactToConstraintKernel"); + launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL)); //launcher.setConst( cdata ); - + launcher.setConst(cdata.m_nContacts); launcher.setConst(cdata.m_dt); launcher.setConst(cdata.m_positionDrift); launcher.setConst(cdata.m_positionConstraintCoeff); - - launcher.launch1D( nContacts, 64 ); - clFinish(m_queue); + launcher.launch1D(nContacts, 64); + clFinish(m_queue); } } - - } /* @@ -1115,28 +1030,24 @@ void b3Solver::sortContacts( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, } */ -void b3Solver::batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* nNative, b3OpenCLArray<unsigned int>* offsetsNative, int staticIdx ) +void b3Solver::batchContacts(b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* nNative, b3OpenCLArray<unsigned int>* offsetsNative, int staticIdx) { - - int numWorkItems = 64*B3_SOLVER_N_CELLS; + int numWorkItems = 64 * B3_SOLVER_N_CELLS; { B3_PROFILE("batch generation"); - + b3Int4 cdata; cdata.x = nContacts; cdata.y = 0; cdata.z = staticIdx; - #ifdef BATCH_DEBUG - SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; - adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device,numWorkItems); - memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems); - gpuDebugInfo.write(debugInfo,numWorkItems); + SolverDebugInfo* debugInfo = new SolverDebugInfo[numWorkItems]; + adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device, numWorkItems); + memset(debugInfo, 0, sizeof(SolverDebugInfo) * numWorkItems); + gpuDebugInfo.write(debugInfo, numWorkItems); #endif - - #if 0 b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contacts->getBufferCL() ), @@ -1148,8 +1059,6 @@ void b3Solver::batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContact #endif }; #endif - - { m_batchSizes.resize(nNative->size()); @@ -1157,22 +1066,21 @@ void b3Solver::batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContact //b3LauncherCL launcher( m_queue, m_batchingKernel); cl_kernel k = useNewBatchingKernel ? m_batchingKernelNew : m_batchingKernel; - b3LauncherCL launcher( m_queue, k,"*batchingKernel"); - if (!useNewBatchingKernel ) + b3LauncherCL launcher(m_queue, k, "*batchingKernel"); + if (!useNewBatchingKernel) { - launcher.setBuffer( contacts->getBufferCL() ); + launcher.setBuffer(contacts->getBufferCL()); } - launcher.setBuffer( m_contactBuffer2->getBufferCL() ); - launcher.setBuffer( nNative->getBufferCL()); - launcher.setBuffer( offsetsNative->getBufferCL()); - + launcher.setBuffer(m_contactBuffer2->getBufferCL()); + launcher.setBuffer(nNative->getBufferCL()); + launcher.setBuffer(offsetsNative->getBufferCL()); + launcher.setBuffer(m_batchSizes.getBufferCL()); - //launcher.setConst( cdata ); - launcher.setConst(staticIdx); - - launcher.launch1D( numWorkItems, 64 ); + launcher.setConst(staticIdx); + + launcher.launch1D(numWorkItems, 64); //clFinish(m_queue); //b3AlignedObjectArray<int> batchSizesCPU; //m_batchSizes.copyToHost(batchSizesCPU); @@ -1180,46 +1088,41 @@ void b3Solver::batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContact } #ifdef BATCH_DEBUG - aaaa - b3Contact4* hostContacts = new b3Contact4[nContacts]; - m_contactBuffer->read(hostContacts,nContacts); + aaaa + b3Contact4* hostContacts = new b3Contact4[nContacts]; + m_contactBuffer->read(hostContacts, nContacts); clFinish(m_queue); - gpuDebugInfo.read(debugInfo,numWorkItems); + gpuDebugInfo.read(debugInfo, numWorkItems); clFinish(m_queue); - for (int i=0;i<numWorkItems;i++) + for (int i = 0; i < numWorkItems; i++) { - if (debugInfo[i].m_valInt1>0) + if (debugInfo[i].m_valInt1 > 0) { printf("catch\n"); } - if (debugInfo[i].m_valInt2>0) + if (debugInfo[i].m_valInt2 > 0) { printf("catch22\n"); } - if (debugInfo[i].m_valInt3>0) + if (debugInfo[i].m_valInt3 > 0) { printf("catch666\n"); } - if (debugInfo[i].m_valInt4>0) + if (debugInfo[i].m_valInt4 > 0) { printf("catch777\n"); } } delete[] debugInfo; -#endif //BATCH_DEBUG - +#endif //BATCH_DEBUG } -// copy buffer to buffer + // copy buffer to buffer //b3Assert(m_contactBuffer->size()==nContacts); //contacts->copyFromOpenCLArray( *m_contactBuffer); //clFinish(m_queue);//needed? - - - } - diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.h index b37f2f1bec..ee63531d78 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.h @@ -13,7 +13,6 @@ subject to the following restrictions: */ //Originally written by Takahiro Harada - #ifndef __ADL_SOLVER_H #define __ADL_SOLVER_H @@ -29,98 +28,83 @@ subject to the following restrictions: #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" - -#define B3NEXTMULTIPLEOF(num, alignment) (((num)/(alignment) + (((num)%(alignment)==0)?0:1))*(alignment)) +#define B3NEXTMULTIPLEOF(num, alignment) (((num) / (alignment) + (((num) % (alignment) == 0) ? 0 : 1)) * (alignment)) enum { - B3_SOLVER_N_SPLIT_X = 8,//16,//4, - B3_SOLVER_N_SPLIT_Y = 4,//16,//4, - B3_SOLVER_N_SPLIT_Z = 8,//, - B3_SOLVER_N_CELLS = B3_SOLVER_N_SPLIT_X*B3_SOLVER_N_SPLIT_Y*B3_SOLVER_N_SPLIT_Z, - B3_SOLVER_N_BATCHES = 8,//4,//8,//4, + B3_SOLVER_N_SPLIT_X = 8, //16,//4, + B3_SOLVER_N_SPLIT_Y = 4, //16,//4, + B3_SOLVER_N_SPLIT_Z = 8, //, + B3_SOLVER_N_CELLS = B3_SOLVER_N_SPLIT_X * B3_SOLVER_N_SPLIT_Y * B3_SOLVER_N_SPLIT_Z, + B3_SOLVER_N_BATCHES = 8, //4,//8,//4, B3_MAX_NUM_BATCHES = 128, }; class b3SolverBase { - public: - - - struct ConstraintCfg - { - ConstraintCfg( float dt = 0.f ): m_positionDrift( 0.005f ), m_positionConstraintCoeff( 0.2f ), m_dt(dt), m_staticIdx(-1) {} - - float m_positionDrift; - float m_positionConstraintCoeff; - float m_dt; - bool m_enableParallelSolve; - float m_batchCellSize; - int m_staticIdx; - }; - +public: + struct ConstraintCfg + { + ConstraintCfg(float dt = 0.f) : m_positionDrift(0.005f), m_positionConstraintCoeff(0.2f), m_dt(dt), m_staticIdx(-1) {} + + float m_positionDrift; + float m_positionConstraintCoeff; + float m_dt; + bool m_enableParallelSolve; + float m_batchCellSize; + int m_staticIdx; + }; }; class b3Solver : public b3SolverBase { - public: - - cl_context m_context; - cl_device_id m_device; - cl_command_queue m_queue; - - - b3OpenCLArray<unsigned int>* m_numConstraints; - b3OpenCLArray<unsigned int>* m_offsets; - b3OpenCLArray<int> m_batchSizes; - - - int m_nIterations; - cl_kernel m_batchingKernel; - cl_kernel m_batchingKernelNew; - cl_kernel m_solveContactKernel; - cl_kernel m_solveFrictionKernel; - cl_kernel m_contactToConstraintKernel; - cl_kernel m_setSortDataKernel; - cl_kernel m_reorderContactKernel; - cl_kernel m_copyConstraintKernel; +public: + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; - class b3RadixSort32CL* m_sort32; - class b3BoundSearchCL* m_search; - class b3PrefixScanCL* m_scan; + b3OpenCLArray<unsigned int>* m_numConstraints; + b3OpenCLArray<unsigned int>* m_offsets; + b3OpenCLArray<int> m_batchSizes; - b3OpenCLArray<b3SortData>* m_sortDataBuffer; - b3OpenCLArray<b3Contact4>* m_contactBuffer2; + int m_nIterations; + cl_kernel m_batchingKernel; + cl_kernel m_batchingKernelNew; + cl_kernel m_solveContactKernel; + cl_kernel m_solveFrictionKernel; + cl_kernel m_contactToConstraintKernel; + cl_kernel m_setSortDataKernel; + cl_kernel m_reorderContactKernel; + cl_kernel m_copyConstraintKernel; - enum - { - DYNAMIC_CONTACT_ALLOCATION_THRESHOLD = 2000000, - }; + class b3RadixSort32CL* m_sort32; + class b3BoundSearchCL* m_search; + class b3PrefixScanCL* m_scan; - + b3OpenCLArray<b3SortData>* m_sortDataBuffer; + b3OpenCLArray<b3Contact4>* m_contactBuffer2; - - b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity); + enum + { + DYNAMIC_CONTACT_ALLOCATION_THRESHOLD = 2000000, + }; - virtual ~b3Solver(); - - void solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* inertiaBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches); + b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity); - void solveContactConstraintHost( b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches, b3AlignedObjectArray<int>* batchSizes); + virtual ~b3Solver(); + void solveContactConstraint(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* inertiaBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches); - void convertToConstraints( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, - const b3OpenCLArray<b3InertiaData>* shapeBuf, - b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData, - int nContacts, const ConstraintCfg& cfg ); + void solveContactConstraintHost(b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n, int maxNumBatches, b3AlignedObjectArray<int>* batchSizes); - void batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* n, b3OpenCLArray<unsigned int>* offsets, int staticIdx ); + void convertToConstraints(const b3OpenCLArray<b3RigidBodyData>* bodyBuf, + const b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData, + int nContacts, const ConstraintCfg& cfg); + void batchContacts(b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* n, b3OpenCLArray<unsigned int>* offsets, int staticIdx); }; - - - -#endif //__ADL_SOLVER_H +#endif //__ADL_SOLVER_H diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h index 150eedc94b..7c73c96baa 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h @@ -1,388 +1,387 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* batchingKernelsCL= \ -"/*\n" -"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Takahiro Harada\n" -"#ifndef B3_CONTACT4DATA_H\n" -"#define B3_CONTACT4DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#define B3_FLOAT4_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#define B3_PLATFORM_DEFINITIONS_H\n" -"struct MyTest\n" -"{\n" -" int bla;\n" -"};\n" -"#ifdef __cplusplus\n" -"#else\n" -"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" -"#define B3_LARGE_FLOAT 1e18f\n" -"#define B3_INFINITY 1e18f\n" -"#define b3Assert(a)\n" -"#define b3ConstArray(a) __global const a*\n" -"#define b3AtomicInc atomic_inc\n" -"#define b3AtomicAdd atomic_add\n" -"#define b3Fabs fabs\n" -"#define b3Sqrt native_sqrt\n" -"#define b3Sin native_sin\n" -"#define b3Cos native_cos\n" -"#define B3_STATIC\n" -"#endif\n" -"#endif\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Float4;\n" -" #define b3Float4ConstArg const b3Float4\n" -" #define b3MakeFloat4 (float4)\n" -" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return dot(a1, b1);\n" -" }\n" -" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return cross(a1, b1);\n" -" }\n" -" #define b3MinFloat4 min\n" -" #define b3MaxFloat4 max\n" -" #define b3Normalized(a) normalize(a)\n" -"#endif \n" -" \n" -"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" -"{\n" -" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" -" return false;\n" -" return true;\n" -"}\n" -"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" -"{\n" -" float maxDot = -B3_INFINITY;\n" -" int i = 0;\n" -" int ptIndex = -1;\n" -" for( i = 0; i < vecLen; i++ )\n" -" {\n" -" float dot = b3Dot3F4(vecArray[i],vec);\n" -" \n" -" if( dot > maxDot )\n" -" {\n" -" maxDot = dot;\n" -" ptIndex = i;\n" -" }\n" -" }\n" -" b3Assert(ptIndex>=0);\n" -" if (ptIndex<0)\n" -" {\n" -" ptIndex = 0;\n" -" }\n" -" *dotOut = maxDot;\n" -" return ptIndex;\n" -"}\n" -"#endif //B3_FLOAT4_H\n" -"typedef struct b3Contact4Data b3Contact4Data_t;\n" -"struct b3Contact4Data\n" -"{\n" -" b3Float4 m_worldPosB[4];\n" -"// b3Float4 m_localPosA[4];\n" -"// b3Float4 m_localPosB[4];\n" -" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" -" unsigned short m_restituitionCoeffCmp;\n" -" unsigned short m_frictionCoeffCmp;\n" -" int m_batchIdx;\n" -" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" -" int m_bodyBPtrAndSignBit;\n" -" int m_childIndexA;\n" -" int m_childIndexB;\n" -" int m_unused1;\n" -" int m_unused2;\n" -"};\n" -"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" -"{\n" -" return (int)contact->m_worldNormalOnB.w;\n" -"};\n" -"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" -"{\n" -" contact->m_worldNormalOnB.w = (float)numPoints;\n" -"};\n" -"#endif //B3_CONTACT4DATA_H\n" -"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" -"#ifdef cl_ext_atomic_counters_32\n" -"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" -"#else\n" -"#define counter32_t volatile __global int*\n" -"#endif\n" -"typedef unsigned int u32;\n" -"typedef unsigned short u16;\n" -"typedef unsigned char u8;\n" -"#define GET_GROUP_IDX get_group_id(0)\n" -"#define GET_LOCAL_IDX get_local_id(0)\n" -"#define GET_GLOBAL_IDX get_global_id(0)\n" -"#define GET_GROUP_SIZE get_local_size(0)\n" -"#define GET_NUM_GROUPS get_num_groups(0)\n" -"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" -"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" -"#define AtomInc(x) atom_inc(&(x))\n" -"#define AtomInc1(x, out) out = atom_inc(&(x))\n" -"#define AppendInc(x, out) out = atomic_inc(x)\n" -"#define AtomAdd(x, value) atom_add(&(x), value)\n" -"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" -"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" -"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" -"#define make_float4 (float4)\n" -"#define make_float2 (float2)\n" -"#define make_uint4 (uint4)\n" -"#define make_int4 (int4)\n" -"#define make_uint2 (uint2)\n" -"#define make_int2 (int2)\n" -"#define max2 max\n" -"#define min2 min\n" -"#define WG_SIZE 64\n" -"typedef struct \n" -"{\n" -" int m_n;\n" -" int m_start;\n" -" int m_staticIdx;\n" -" int m_paddings[1];\n" -"} ConstBuffer;\n" -"typedef struct \n" -"{\n" -" int m_a;\n" -" int m_b;\n" -" u32 m_idx;\n" -"}Elem;\n" -"#define STACK_SIZE (WG_SIZE*10)\n" -"//#define STACK_SIZE (WG_SIZE)\n" -"#define RING_SIZE 1024\n" -"#define RING_SIZE_MASK (RING_SIZE-1)\n" -"#define CHECK_SIZE (WG_SIZE)\n" -"#define GET_RING_CAPACITY (RING_SIZE - ldsRingEnd)\n" -"#define RING_END ldsTmp\n" -"u32 readBuf(__local u32* buff, int idx)\n" -"{\n" -" idx = idx % (32*CHECK_SIZE);\n" -" int bitIdx = idx%32;\n" -" int bufIdx = idx/32;\n" -" return buff[bufIdx] & (1<<bitIdx);\n" -"}\n" -"void writeBuf(__local u32* buff, int idx)\n" -"{\n" -" idx = idx % (32*CHECK_SIZE);\n" -" int bitIdx = idx%32;\n" -" int bufIdx = idx/32;\n" -"// buff[bufIdx] |= (1<<bitIdx);\n" -" atom_or( &buff[bufIdx], (1<<bitIdx) );\n" -"}\n" -"u32 tryWrite(__local u32* buff, int idx)\n" -"{\n" -" idx = idx % (32*CHECK_SIZE);\n" -" int bitIdx = idx%32;\n" -" int bufIdx = idx/32;\n" -" u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) );\n" -" return ((ans >> bitIdx)&1) == 0;\n" -"}\n" -"// batching on the GPU\n" -"__kernel void CreateBatches( __global const struct b3Contact4Data* gConstraints, __global struct b3Contact4Data* gConstraintsOut,\n" -" __global const u32* gN, __global const u32* gStart, __global int* batchSizes, \n" -" int m_staticIdx )\n" -"{\n" -" __local u32 ldsStackIdx[STACK_SIZE];\n" -" __local u32 ldsStackEnd;\n" -" __local Elem ldsRingElem[RING_SIZE];\n" -" __local u32 ldsRingEnd;\n" -" __local u32 ldsTmp;\n" -" __local u32 ldsCheckBuffer[CHECK_SIZE];\n" -" __local u32 ldsFixedBuffer[CHECK_SIZE];\n" -" __local u32 ldsGEnd;\n" -" __local u32 ldsDstEnd;\n" -" int wgIdx = GET_GROUP_IDX;\n" -" int lIdx = GET_LOCAL_IDX;\n" -" \n" -" const int m_n = gN[wgIdx];\n" -" const int m_start = gStart[wgIdx];\n" -" \n" -" if( lIdx == 0 )\n" -" {\n" -" ldsRingEnd = 0;\n" -" ldsGEnd = 0;\n" -" ldsStackEnd = 0;\n" -" ldsDstEnd = m_start;\n" -" }\n" -" \n" -" \n" -" \n" -"// while(1)\n" -"//was 250\n" -" int ie=0;\n" -" int maxBatch = 0;\n" -" for(ie=0; ie<50; ie++)\n" -" {\n" -" ldsFixedBuffer[lIdx] = 0;\n" -" for(int giter=0; giter<4; giter++)\n" -" {\n" -" int ringCap = GET_RING_CAPACITY;\n" -" \n" -" // 1. fill ring\n" -" if( ldsGEnd < m_n )\n" -" {\n" -" while( ringCap > WG_SIZE )\n" -" {\n" -" if( ldsGEnd >= m_n ) break;\n" -" if( lIdx < ringCap - WG_SIZE )\n" -" {\n" -" int srcIdx;\n" -" AtomInc1( ldsGEnd, srcIdx );\n" -" if( srcIdx < m_n )\n" -" {\n" -" int dstIdx;\n" -" AtomInc1( ldsRingEnd, dstIdx );\n" -" \n" -" int a = gConstraints[m_start+srcIdx].m_bodyAPtrAndSignBit;\n" -" int b = gConstraints[m_start+srcIdx].m_bodyBPtrAndSignBit;\n" -" ldsRingElem[dstIdx].m_a = (a>b)? b:a;\n" -" ldsRingElem[dstIdx].m_b = (a>b)? a:b;\n" -" ldsRingElem[dstIdx].m_idx = srcIdx;\n" -" }\n" -" }\n" -" ringCap = GET_RING_CAPACITY;\n" -" }\n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" \n" -" // 2. fill stack\n" -" __local Elem* dst = ldsRingElem;\n" -" if( lIdx == 0 ) RING_END = 0;\n" -" int srcIdx=lIdx;\n" -" int end = ldsRingEnd;\n" -" {\n" -" for(int ii=0; ii<end; ii+=WG_SIZE, srcIdx+=WG_SIZE)\n" -" {\n" -" Elem e;\n" -" if(srcIdx<end) e = ldsRingElem[srcIdx];\n" -" bool done = (srcIdx<end)?false:true;\n" -" for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) ldsCheckBuffer[lIdx] = 0;\n" -" \n" -" if( !done )\n" -" {\n" -" int aUsed = readBuf( ldsFixedBuffer, abs(e.m_a));\n" -" int bUsed = readBuf( ldsFixedBuffer, abs(e.m_b));\n" -" if( aUsed==0 && bUsed==0 )\n" -" {\n" -" int aAvailable=1;\n" -" int bAvailable=1;\n" -" int ea = abs(e.m_a);\n" -" int eb = abs(e.m_b);\n" -" bool aStatic = (e.m_a<0) ||(ea==m_staticIdx);\n" -" bool bStatic = (e.m_b<0) ||(eb==m_staticIdx);\n" -" \n" -" if (!aStatic)\n" -" aAvailable = tryWrite( ldsCheckBuffer, ea );\n" -" if (!bStatic)\n" -" bAvailable = tryWrite( ldsCheckBuffer, eb );\n" -" \n" -" //aAvailable = aStatic? 1: aAvailable;\n" -" //bAvailable = bStatic? 1: bAvailable;\n" -" bool success = (aAvailable && bAvailable);\n" -" if(success)\n" -" {\n" -" \n" -" if (!aStatic)\n" -" writeBuf( ldsFixedBuffer, ea );\n" -" if (!bStatic)\n" -" writeBuf( ldsFixedBuffer, eb );\n" -" }\n" -" done = success;\n" -" }\n" -" }\n" -" // put it aside\n" -" if(srcIdx<end)\n" -" {\n" -" if( done )\n" -" {\n" -" int dstIdx; AtomInc1( ldsStackEnd, dstIdx );\n" -" if( dstIdx < STACK_SIZE )\n" -" ldsStackIdx[dstIdx] = e.m_idx;\n" -" else{\n" -" done = false;\n" -" AtomAdd( ldsStackEnd, -1 );\n" -" }\n" -" }\n" -" if( !done )\n" -" {\n" -" int dstIdx; AtomInc1( RING_END, dstIdx );\n" -" dst[dstIdx] = e;\n" -" }\n" -" }\n" -" // if filled, flush\n" -" if( ldsStackEnd == STACK_SIZE )\n" -" {\n" -" for(int i=lIdx; i<STACK_SIZE; i+=WG_SIZE)\n" -" {\n" -" int idx = m_start + ldsStackIdx[i];\n" -" int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n" -" gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n" -" gConstraintsOut[ dstIdx ].m_batchIdx = ie;\n" -" }\n" -" if( lIdx == 0 ) ldsStackEnd = 0;\n" -" //for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) \n" -" ldsFixedBuffer[lIdx] = 0;\n" -" }\n" -" }\n" -" }\n" -" if( lIdx == 0 ) ldsRingEnd = RING_END;\n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" for(int i=lIdx; i<ldsStackEnd; i+=WG_SIZE)\n" -" {\n" -" int idx = m_start + ldsStackIdx[i];\n" -" int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n" -" gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n" -" gConstraintsOut[ dstIdx ].m_batchIdx = ie;\n" -" }\n" -" // in case it couldn't consume any pair. Flush them\n" -" // todo. Serial batch worth while?\n" -" if( ldsStackEnd == 0 )\n" -" {\n" -" for(int i=lIdx; i<ldsRingEnd; i+=WG_SIZE)\n" -" {\n" -" int idx = m_start + ldsRingElem[i].m_idx;\n" -" int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n" -" gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n" -" int curBatch = 100+i;\n" -" if (maxBatch < curBatch)\n" -" maxBatch = curBatch;\n" -" \n" -" gConstraintsOut[ dstIdx ].m_batchIdx = curBatch;\n" -" \n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" if( lIdx == 0 ) ldsRingEnd = 0;\n" -" }\n" -" if( lIdx == 0 ) ldsStackEnd = 0;\n" -" GROUP_LDS_BARRIER;\n" -" // termination\n" -" if( ldsGEnd == m_n && ldsRingEnd == 0 )\n" -" break;\n" -" }\n" -" if( lIdx == 0 )\n" -" {\n" -" if (maxBatch < ie)\n" -" maxBatch=ie;\n" -" batchSizes[wgIdx]=maxBatch;\n" -" }\n" -"}\n" -; +static const char* batchingKernelsCL = + "/*\n" + "Copyright (c) 2012 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Takahiro Harada\n" + "#ifndef B3_CONTACT4DATA_H\n" + "#define B3_CONTACT4DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#define B3_FLOAT4_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#define B3_PLATFORM_DEFINITIONS_H\n" + "struct MyTest\n" + "{\n" + " int bla;\n" + "};\n" + "#ifdef __cplusplus\n" + "#else\n" + "//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" + "#define B3_LARGE_FLOAT 1e18f\n" + "#define B3_INFINITY 1e18f\n" + "#define b3Assert(a)\n" + "#define b3ConstArray(a) __global const a*\n" + "#define b3AtomicInc atomic_inc\n" + "#define b3AtomicAdd atomic_add\n" + "#define b3Fabs fabs\n" + "#define b3Sqrt native_sqrt\n" + "#define b3Sin native_sin\n" + "#define b3Cos native_cos\n" + "#define B3_STATIC\n" + "#endif\n" + "#endif\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Float4;\n" + " #define b3Float4ConstArg const b3Float4\n" + " #define b3MakeFloat4 (float4)\n" + " float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return dot(a1, b1);\n" + " }\n" + " b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return cross(a1, b1);\n" + " }\n" + " #define b3MinFloat4 min\n" + " #define b3MaxFloat4 max\n" + " #define b3Normalized(a) normalize(a)\n" + "#endif \n" + " \n" + "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" + "{\n" + " if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" + " return false;\n" + " return true;\n" + "}\n" + "inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" + "{\n" + " float maxDot = -B3_INFINITY;\n" + " int i = 0;\n" + " int ptIndex = -1;\n" + " for( i = 0; i < vecLen; i++ )\n" + " {\n" + " float dot = b3Dot3F4(vecArray[i],vec);\n" + " \n" + " if( dot > maxDot )\n" + " {\n" + " maxDot = dot;\n" + " ptIndex = i;\n" + " }\n" + " }\n" + " b3Assert(ptIndex>=0);\n" + " if (ptIndex<0)\n" + " {\n" + " ptIndex = 0;\n" + " }\n" + " *dotOut = maxDot;\n" + " return ptIndex;\n" + "}\n" + "#endif //B3_FLOAT4_H\n" + "typedef struct b3Contact4Data b3Contact4Data_t;\n" + "struct b3Contact4Data\n" + "{\n" + " b3Float4 m_worldPosB[4];\n" + "// b3Float4 m_localPosA[4];\n" + "// b3Float4 m_localPosB[4];\n" + " b3Float4 m_worldNormalOnB; // w: m_nPoints\n" + " unsigned short m_restituitionCoeffCmp;\n" + " unsigned short m_frictionCoeffCmp;\n" + " int m_batchIdx;\n" + " int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" + " int m_bodyBPtrAndSignBit;\n" + " int m_childIndexA;\n" + " int m_childIndexB;\n" + " int m_unused1;\n" + " int m_unused2;\n" + "};\n" + "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" + "{\n" + " return (int)contact->m_worldNormalOnB.w;\n" + "};\n" + "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" + "{\n" + " contact->m_worldNormalOnB.w = (float)numPoints;\n" + "};\n" + "#endif //B3_CONTACT4DATA_H\n" + "#pragma OPENCL EXTENSION cl_amd_printf : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" + "#ifdef cl_ext_atomic_counters_32\n" + "#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" + "#else\n" + "#define counter32_t volatile __global int*\n" + "#endif\n" + "typedef unsigned int u32;\n" + "typedef unsigned short u16;\n" + "typedef unsigned char u8;\n" + "#define GET_GROUP_IDX get_group_id(0)\n" + "#define GET_LOCAL_IDX get_local_id(0)\n" + "#define GET_GLOBAL_IDX get_global_id(0)\n" + "#define GET_GROUP_SIZE get_local_size(0)\n" + "#define GET_NUM_GROUPS get_num_groups(0)\n" + "#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" + "#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" + "#define AtomInc(x) atom_inc(&(x))\n" + "#define AtomInc1(x, out) out = atom_inc(&(x))\n" + "#define AppendInc(x, out) out = atomic_inc(x)\n" + "#define AtomAdd(x, value) atom_add(&(x), value)\n" + "#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" + "#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" + "#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" + "#define make_float4 (float4)\n" + "#define make_float2 (float2)\n" + "#define make_uint4 (uint4)\n" + "#define make_int4 (int4)\n" + "#define make_uint2 (uint2)\n" + "#define make_int2 (int2)\n" + "#define max2 max\n" + "#define min2 min\n" + "#define WG_SIZE 64\n" + "typedef struct \n" + "{\n" + " int m_n;\n" + " int m_start;\n" + " int m_staticIdx;\n" + " int m_paddings[1];\n" + "} ConstBuffer;\n" + "typedef struct \n" + "{\n" + " int m_a;\n" + " int m_b;\n" + " u32 m_idx;\n" + "}Elem;\n" + "#define STACK_SIZE (WG_SIZE*10)\n" + "//#define STACK_SIZE (WG_SIZE)\n" + "#define RING_SIZE 1024\n" + "#define RING_SIZE_MASK (RING_SIZE-1)\n" + "#define CHECK_SIZE (WG_SIZE)\n" + "#define GET_RING_CAPACITY (RING_SIZE - ldsRingEnd)\n" + "#define RING_END ldsTmp\n" + "u32 readBuf(__local u32* buff, int idx)\n" + "{\n" + " idx = idx % (32*CHECK_SIZE);\n" + " int bitIdx = idx%32;\n" + " int bufIdx = idx/32;\n" + " return buff[bufIdx] & (1<<bitIdx);\n" + "}\n" + "void writeBuf(__local u32* buff, int idx)\n" + "{\n" + " idx = idx % (32*CHECK_SIZE);\n" + " int bitIdx = idx%32;\n" + " int bufIdx = idx/32;\n" + "// buff[bufIdx] |= (1<<bitIdx);\n" + " atom_or( &buff[bufIdx], (1<<bitIdx) );\n" + "}\n" + "u32 tryWrite(__local u32* buff, int idx)\n" + "{\n" + " idx = idx % (32*CHECK_SIZE);\n" + " int bitIdx = idx%32;\n" + " int bufIdx = idx/32;\n" + " u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) );\n" + " return ((ans >> bitIdx)&1) == 0;\n" + "}\n" + "// batching on the GPU\n" + "__kernel void CreateBatches( __global const struct b3Contact4Data* gConstraints, __global struct b3Contact4Data* gConstraintsOut,\n" + " __global const u32* gN, __global const u32* gStart, __global int* batchSizes, \n" + " int m_staticIdx )\n" + "{\n" + " __local u32 ldsStackIdx[STACK_SIZE];\n" + " __local u32 ldsStackEnd;\n" + " __local Elem ldsRingElem[RING_SIZE];\n" + " __local u32 ldsRingEnd;\n" + " __local u32 ldsTmp;\n" + " __local u32 ldsCheckBuffer[CHECK_SIZE];\n" + " __local u32 ldsFixedBuffer[CHECK_SIZE];\n" + " __local u32 ldsGEnd;\n" + " __local u32 ldsDstEnd;\n" + " int wgIdx = GET_GROUP_IDX;\n" + " int lIdx = GET_LOCAL_IDX;\n" + " \n" + " const int m_n = gN[wgIdx];\n" + " const int m_start = gStart[wgIdx];\n" + " \n" + " if( lIdx == 0 )\n" + " {\n" + " ldsRingEnd = 0;\n" + " ldsGEnd = 0;\n" + " ldsStackEnd = 0;\n" + " ldsDstEnd = m_start;\n" + " }\n" + " \n" + " \n" + " \n" + "// while(1)\n" + "//was 250\n" + " int ie=0;\n" + " int maxBatch = 0;\n" + " for(ie=0; ie<50; ie++)\n" + " {\n" + " ldsFixedBuffer[lIdx] = 0;\n" + " for(int giter=0; giter<4; giter++)\n" + " {\n" + " int ringCap = GET_RING_CAPACITY;\n" + " \n" + " // 1. fill ring\n" + " if( ldsGEnd < m_n )\n" + " {\n" + " while( ringCap > WG_SIZE )\n" + " {\n" + " if( ldsGEnd >= m_n ) break;\n" + " if( lIdx < ringCap - WG_SIZE )\n" + " {\n" + " int srcIdx;\n" + " AtomInc1( ldsGEnd, srcIdx );\n" + " if( srcIdx < m_n )\n" + " {\n" + " int dstIdx;\n" + " AtomInc1( ldsRingEnd, dstIdx );\n" + " \n" + " int a = gConstraints[m_start+srcIdx].m_bodyAPtrAndSignBit;\n" + " int b = gConstraints[m_start+srcIdx].m_bodyBPtrAndSignBit;\n" + " ldsRingElem[dstIdx].m_a = (a>b)? b:a;\n" + " ldsRingElem[dstIdx].m_b = (a>b)? a:b;\n" + " ldsRingElem[dstIdx].m_idx = srcIdx;\n" + " }\n" + " }\n" + " ringCap = GET_RING_CAPACITY;\n" + " }\n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " \n" + " // 2. fill stack\n" + " __local Elem* dst = ldsRingElem;\n" + " if( lIdx == 0 ) RING_END = 0;\n" + " int srcIdx=lIdx;\n" + " int end = ldsRingEnd;\n" + " {\n" + " for(int ii=0; ii<end; ii+=WG_SIZE, srcIdx+=WG_SIZE)\n" + " {\n" + " Elem e;\n" + " if(srcIdx<end) e = ldsRingElem[srcIdx];\n" + " bool done = (srcIdx<end)?false:true;\n" + " for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) ldsCheckBuffer[lIdx] = 0;\n" + " \n" + " if( !done )\n" + " {\n" + " int aUsed = readBuf( ldsFixedBuffer, abs(e.m_a));\n" + " int bUsed = readBuf( ldsFixedBuffer, abs(e.m_b));\n" + " if( aUsed==0 && bUsed==0 )\n" + " {\n" + " int aAvailable=1;\n" + " int bAvailable=1;\n" + " int ea = abs(e.m_a);\n" + " int eb = abs(e.m_b);\n" + " bool aStatic = (e.m_a<0) ||(ea==m_staticIdx);\n" + " bool bStatic = (e.m_b<0) ||(eb==m_staticIdx);\n" + " \n" + " if (!aStatic)\n" + " aAvailable = tryWrite( ldsCheckBuffer, ea );\n" + " if (!bStatic)\n" + " bAvailable = tryWrite( ldsCheckBuffer, eb );\n" + " \n" + " //aAvailable = aStatic? 1: aAvailable;\n" + " //bAvailable = bStatic? 1: bAvailable;\n" + " bool success = (aAvailable && bAvailable);\n" + " if(success)\n" + " {\n" + " \n" + " if (!aStatic)\n" + " writeBuf( ldsFixedBuffer, ea );\n" + " if (!bStatic)\n" + " writeBuf( ldsFixedBuffer, eb );\n" + " }\n" + " done = success;\n" + " }\n" + " }\n" + " // put it aside\n" + " if(srcIdx<end)\n" + " {\n" + " if( done )\n" + " {\n" + " int dstIdx; AtomInc1( ldsStackEnd, dstIdx );\n" + " if( dstIdx < STACK_SIZE )\n" + " ldsStackIdx[dstIdx] = e.m_idx;\n" + " else{\n" + " done = false;\n" + " AtomAdd( ldsStackEnd, -1 );\n" + " }\n" + " }\n" + " if( !done )\n" + " {\n" + " int dstIdx; AtomInc1( RING_END, dstIdx );\n" + " dst[dstIdx] = e;\n" + " }\n" + " }\n" + " // if filled, flush\n" + " if( ldsStackEnd == STACK_SIZE )\n" + " {\n" + " for(int i=lIdx; i<STACK_SIZE; i+=WG_SIZE)\n" + " {\n" + " int idx = m_start + ldsStackIdx[i];\n" + " int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n" + " gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n" + " gConstraintsOut[ dstIdx ].m_batchIdx = ie;\n" + " }\n" + " if( lIdx == 0 ) ldsStackEnd = 0;\n" + " //for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) \n" + " ldsFixedBuffer[lIdx] = 0;\n" + " }\n" + " }\n" + " }\n" + " if( lIdx == 0 ) ldsRingEnd = RING_END;\n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " for(int i=lIdx; i<ldsStackEnd; i+=WG_SIZE)\n" + " {\n" + " int idx = m_start + ldsStackIdx[i];\n" + " int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n" + " gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n" + " gConstraintsOut[ dstIdx ].m_batchIdx = ie;\n" + " }\n" + " // in case it couldn't consume any pair. Flush them\n" + " // todo. Serial batch worth while?\n" + " if( ldsStackEnd == 0 )\n" + " {\n" + " for(int i=lIdx; i<ldsRingEnd; i+=WG_SIZE)\n" + " {\n" + " int idx = m_start + ldsRingElem[i].m_idx;\n" + " int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n" + " gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n" + " int curBatch = 100+i;\n" + " if (maxBatch < curBatch)\n" + " maxBatch = curBatch;\n" + " \n" + " gConstraintsOut[ dstIdx ].m_batchIdx = curBatch;\n" + " \n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " if( lIdx == 0 ) ldsRingEnd = 0;\n" + " }\n" + " if( lIdx == 0 ) ldsStackEnd = 0;\n" + " GROUP_LDS_BARRIER;\n" + " // termination\n" + " if( ldsGEnd == m_n && ldsRingEnd == 0 )\n" + " break;\n" + " }\n" + " if( lIdx == 0 )\n" + " {\n" + " if (maxBatch < ie)\n" + " maxBatch=ie;\n" + " batchSizes[wgIdx]=maxBatch;\n" + " }\n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h index 1e5957adae..05800656cb 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h @@ -1,291 +1,290 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* batchingKernelsNewCL= \ -"/*\n" -"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Erwin Coumans\n" -"#ifndef B3_CONTACT4DATA_H\n" -"#define B3_CONTACT4DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#define B3_FLOAT4_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#define B3_PLATFORM_DEFINITIONS_H\n" -"struct MyTest\n" -"{\n" -" int bla;\n" -"};\n" -"#ifdef __cplusplus\n" -"#else\n" -"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" -"#define B3_LARGE_FLOAT 1e18f\n" -"#define B3_INFINITY 1e18f\n" -"#define b3Assert(a)\n" -"#define b3ConstArray(a) __global const a*\n" -"#define b3AtomicInc atomic_inc\n" -"#define b3AtomicAdd atomic_add\n" -"#define b3Fabs fabs\n" -"#define b3Sqrt native_sqrt\n" -"#define b3Sin native_sin\n" -"#define b3Cos native_cos\n" -"#define B3_STATIC\n" -"#endif\n" -"#endif\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Float4;\n" -" #define b3Float4ConstArg const b3Float4\n" -" #define b3MakeFloat4 (float4)\n" -" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return dot(a1, b1);\n" -" }\n" -" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return cross(a1, b1);\n" -" }\n" -" #define b3MinFloat4 min\n" -" #define b3MaxFloat4 max\n" -" #define b3Normalized(a) normalize(a)\n" -"#endif \n" -" \n" -"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" -"{\n" -" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" -" return false;\n" -" return true;\n" -"}\n" -"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" -"{\n" -" float maxDot = -B3_INFINITY;\n" -" int i = 0;\n" -" int ptIndex = -1;\n" -" for( i = 0; i < vecLen; i++ )\n" -" {\n" -" float dot = b3Dot3F4(vecArray[i],vec);\n" -" \n" -" if( dot > maxDot )\n" -" {\n" -" maxDot = dot;\n" -" ptIndex = i;\n" -" }\n" -" }\n" -" b3Assert(ptIndex>=0);\n" -" if (ptIndex<0)\n" -" {\n" -" ptIndex = 0;\n" -" }\n" -" *dotOut = maxDot;\n" -" return ptIndex;\n" -"}\n" -"#endif //B3_FLOAT4_H\n" -"typedef struct b3Contact4Data b3Contact4Data_t;\n" -"struct b3Contact4Data\n" -"{\n" -" b3Float4 m_worldPosB[4];\n" -"// b3Float4 m_localPosA[4];\n" -"// b3Float4 m_localPosB[4];\n" -" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" -" unsigned short m_restituitionCoeffCmp;\n" -" unsigned short m_frictionCoeffCmp;\n" -" int m_batchIdx;\n" -" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" -" int m_bodyBPtrAndSignBit;\n" -" int m_childIndexA;\n" -" int m_childIndexB;\n" -" int m_unused1;\n" -" int m_unused2;\n" -"};\n" -"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" -"{\n" -" return (int)contact->m_worldNormalOnB.w;\n" -"};\n" -"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" -"{\n" -" contact->m_worldNormalOnB.w = (float)numPoints;\n" -"};\n" -"#endif //B3_CONTACT4DATA_H\n" -"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" -"#ifdef cl_ext_atomic_counters_32\n" -"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" -"#else\n" -"#define counter32_t volatile __global int*\n" -"#endif\n" -"#define SIMD_WIDTH 64\n" -"typedef unsigned int u32;\n" -"typedef unsigned short u16;\n" -"typedef unsigned char u8;\n" -"#define GET_GROUP_IDX get_group_id(0)\n" -"#define GET_LOCAL_IDX get_local_id(0)\n" -"#define GET_GLOBAL_IDX get_global_id(0)\n" -"#define GET_GROUP_SIZE get_local_size(0)\n" -"#define GET_NUM_GROUPS get_num_groups(0)\n" -"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" -"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" -"#define AtomInc(x) atom_inc(&(x))\n" -"#define AtomInc1(x, out) out = atom_inc(&(x))\n" -"#define AppendInc(x, out) out = atomic_inc(x)\n" -"#define AtomAdd(x, value) atom_add(&(x), value)\n" -"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" -"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" -"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" -"#define make_float4 (float4)\n" -"#define make_float2 (float2)\n" -"#define make_uint4 (uint4)\n" -"#define make_int4 (int4)\n" -"#define make_uint2 (uint2)\n" -"#define make_int2 (int2)\n" -"#define max2 max\n" -"#define min2 min\n" -"#define WG_SIZE 64\n" -"typedef struct \n" -"{\n" -" int m_n;\n" -" int m_start;\n" -" int m_staticIdx;\n" -" int m_paddings[1];\n" -"} ConstBuffer;\n" -"typedef struct \n" -"{\n" -" int m_a;\n" -" int m_b;\n" -" u32 m_idx;\n" -"}Elem;\n" -"// batching on the GPU\n" -"__kernel void CreateBatchesBruteForce( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, int m_staticIdx )\n" -"{\n" -" int wgIdx = GET_GROUP_IDX;\n" -" int lIdx = GET_LOCAL_IDX;\n" -" \n" -" const int m_n = gN[wgIdx];\n" -" const int m_start = gStart[wgIdx];\n" -" \n" -" if( lIdx == 0 )\n" -" {\n" -" for (int i=0;i<m_n;i++)\n" -" {\n" -" int srcIdx = i+m_start;\n" -" int batchIndex = i;\n" -" gConstraints[ srcIdx ].m_batchIdx = batchIndex; \n" -" }\n" -" }\n" -"}\n" -"#define CHECK_SIZE (WG_SIZE)\n" -"u32 readBuf(__local u32* buff, int idx)\n" -"{\n" -" idx = idx % (32*CHECK_SIZE);\n" -" int bitIdx = idx%32;\n" -" int bufIdx = idx/32;\n" -" return buff[bufIdx] & (1<<bitIdx);\n" -"}\n" -"void writeBuf(__local u32* buff, int idx)\n" -"{\n" -" idx = idx % (32*CHECK_SIZE);\n" -" int bitIdx = idx%32;\n" -" int bufIdx = idx/32;\n" -" buff[bufIdx] |= (1<<bitIdx);\n" -" //atom_or( &buff[bufIdx], (1<<bitIdx) );\n" -"}\n" -"u32 tryWrite(__local u32* buff, int idx)\n" -"{\n" -" idx = idx % (32*CHECK_SIZE);\n" -" int bitIdx = idx%32;\n" -" int bufIdx = idx/32;\n" -" u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) );\n" -" return ((ans >> bitIdx)&1) == 0;\n" -"}\n" -"// batching on the GPU\n" -"__kernel void CreateBatchesNew( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, __global int* batchSizes, int staticIdx )\n" -"{\n" -" int wgIdx = GET_GROUP_IDX;\n" -" int lIdx = GET_LOCAL_IDX;\n" -" const int numConstraints = gN[wgIdx];\n" -" const int m_start = gStart[wgIdx];\n" -" b3Contact4Data_t tmp;\n" -" \n" -" __local u32 ldsFixedBuffer[CHECK_SIZE];\n" -" \n" -" \n" -" \n" -" \n" -" \n" -" if( lIdx == 0 )\n" -" {\n" -" \n" -" \n" -" __global struct b3Contact4Data* cs = &gConstraints[m_start]; \n" -" \n" -" \n" -" int numValidConstraints = 0;\n" -" int batchIdx = 0;\n" -" while( numValidConstraints < numConstraints)\n" -" {\n" -" int nCurrentBatch = 0;\n" -" // clear flag\n" -" \n" -" for(int i=0; i<CHECK_SIZE; i++) \n" -" ldsFixedBuffer[i] = 0; \n" -" for(int i=numValidConstraints; i<numConstraints; i++)\n" -" {\n" -" int bodyAS = cs[i].m_bodyAPtrAndSignBit;\n" -" int bodyBS = cs[i].m_bodyBPtrAndSignBit;\n" -" int bodyA = abs(bodyAS);\n" -" int bodyB = abs(bodyBS);\n" -" bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx;\n" -" bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx;\n" -" int aUnavailable = aIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyA);\n" -" int bUnavailable = bIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyB);\n" -" \n" -" if( aUnavailable==0 && bUnavailable==0 ) // ok\n" -" {\n" -" if (!aIsStatic)\n" -" {\n" -" writeBuf( ldsFixedBuffer, bodyA );\n" -" }\n" -" if (!bIsStatic)\n" -" {\n" -" writeBuf( ldsFixedBuffer, bodyB );\n" -" }\n" -" cs[i].m_batchIdx = batchIdx;\n" -" if (i!=numValidConstraints)\n" -" {\n" -" tmp = cs[i];\n" -" cs[i] = cs[numValidConstraints];\n" -" cs[numValidConstraints] = tmp;\n" -" }\n" -" numValidConstraints++;\n" -" \n" -" nCurrentBatch++;\n" -" if( nCurrentBatch == SIMD_WIDTH)\n" -" {\n" -" nCurrentBatch = 0;\n" -" for(int i=0; i<CHECK_SIZE; i++) \n" -" ldsFixedBuffer[i] = 0;\n" -" \n" -" }\n" -" }\n" -" }//for\n" -" batchIdx ++;\n" -" }//while\n" -" \n" -" batchSizes[wgIdx] = batchIdx;\n" -" }//if( lIdx == 0 )\n" -" \n" -" //return batchIdx;\n" -"}\n" -; +static const char* batchingKernelsNewCL = + "/*\n" + "Copyright (c) 2012 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Erwin Coumans\n" + "#ifndef B3_CONTACT4DATA_H\n" + "#define B3_CONTACT4DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#define B3_FLOAT4_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#define B3_PLATFORM_DEFINITIONS_H\n" + "struct MyTest\n" + "{\n" + " int bla;\n" + "};\n" + "#ifdef __cplusplus\n" + "#else\n" + "//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" + "#define B3_LARGE_FLOAT 1e18f\n" + "#define B3_INFINITY 1e18f\n" + "#define b3Assert(a)\n" + "#define b3ConstArray(a) __global const a*\n" + "#define b3AtomicInc atomic_inc\n" + "#define b3AtomicAdd atomic_add\n" + "#define b3Fabs fabs\n" + "#define b3Sqrt native_sqrt\n" + "#define b3Sin native_sin\n" + "#define b3Cos native_cos\n" + "#define B3_STATIC\n" + "#endif\n" + "#endif\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Float4;\n" + " #define b3Float4ConstArg const b3Float4\n" + " #define b3MakeFloat4 (float4)\n" + " float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return dot(a1, b1);\n" + " }\n" + " b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return cross(a1, b1);\n" + " }\n" + " #define b3MinFloat4 min\n" + " #define b3MaxFloat4 max\n" + " #define b3Normalized(a) normalize(a)\n" + "#endif \n" + " \n" + "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" + "{\n" + " if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" + " return false;\n" + " return true;\n" + "}\n" + "inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" + "{\n" + " float maxDot = -B3_INFINITY;\n" + " int i = 0;\n" + " int ptIndex = -1;\n" + " for( i = 0; i < vecLen; i++ )\n" + " {\n" + " float dot = b3Dot3F4(vecArray[i],vec);\n" + " \n" + " if( dot > maxDot )\n" + " {\n" + " maxDot = dot;\n" + " ptIndex = i;\n" + " }\n" + " }\n" + " b3Assert(ptIndex>=0);\n" + " if (ptIndex<0)\n" + " {\n" + " ptIndex = 0;\n" + " }\n" + " *dotOut = maxDot;\n" + " return ptIndex;\n" + "}\n" + "#endif //B3_FLOAT4_H\n" + "typedef struct b3Contact4Data b3Contact4Data_t;\n" + "struct b3Contact4Data\n" + "{\n" + " b3Float4 m_worldPosB[4];\n" + "// b3Float4 m_localPosA[4];\n" + "// b3Float4 m_localPosB[4];\n" + " b3Float4 m_worldNormalOnB; // w: m_nPoints\n" + " unsigned short m_restituitionCoeffCmp;\n" + " unsigned short m_frictionCoeffCmp;\n" + " int m_batchIdx;\n" + " int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" + " int m_bodyBPtrAndSignBit;\n" + " int m_childIndexA;\n" + " int m_childIndexB;\n" + " int m_unused1;\n" + " int m_unused2;\n" + "};\n" + "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" + "{\n" + " return (int)contact->m_worldNormalOnB.w;\n" + "};\n" + "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" + "{\n" + " contact->m_worldNormalOnB.w = (float)numPoints;\n" + "};\n" + "#endif //B3_CONTACT4DATA_H\n" + "#pragma OPENCL EXTENSION cl_amd_printf : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" + "#ifdef cl_ext_atomic_counters_32\n" + "#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" + "#else\n" + "#define counter32_t volatile __global int*\n" + "#endif\n" + "#define SIMD_WIDTH 64\n" + "typedef unsigned int u32;\n" + "typedef unsigned short u16;\n" + "typedef unsigned char u8;\n" + "#define GET_GROUP_IDX get_group_id(0)\n" + "#define GET_LOCAL_IDX get_local_id(0)\n" + "#define GET_GLOBAL_IDX get_global_id(0)\n" + "#define GET_GROUP_SIZE get_local_size(0)\n" + "#define GET_NUM_GROUPS get_num_groups(0)\n" + "#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" + "#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" + "#define AtomInc(x) atom_inc(&(x))\n" + "#define AtomInc1(x, out) out = atom_inc(&(x))\n" + "#define AppendInc(x, out) out = atomic_inc(x)\n" + "#define AtomAdd(x, value) atom_add(&(x), value)\n" + "#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" + "#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" + "#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" + "#define make_float4 (float4)\n" + "#define make_float2 (float2)\n" + "#define make_uint4 (uint4)\n" + "#define make_int4 (int4)\n" + "#define make_uint2 (uint2)\n" + "#define make_int2 (int2)\n" + "#define max2 max\n" + "#define min2 min\n" + "#define WG_SIZE 64\n" + "typedef struct \n" + "{\n" + " int m_n;\n" + " int m_start;\n" + " int m_staticIdx;\n" + " int m_paddings[1];\n" + "} ConstBuffer;\n" + "typedef struct \n" + "{\n" + " int m_a;\n" + " int m_b;\n" + " u32 m_idx;\n" + "}Elem;\n" + "// batching on the GPU\n" + "__kernel void CreateBatchesBruteForce( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, int m_staticIdx )\n" + "{\n" + " int wgIdx = GET_GROUP_IDX;\n" + " int lIdx = GET_LOCAL_IDX;\n" + " \n" + " const int m_n = gN[wgIdx];\n" + " const int m_start = gStart[wgIdx];\n" + " \n" + " if( lIdx == 0 )\n" + " {\n" + " for (int i=0;i<m_n;i++)\n" + " {\n" + " int srcIdx = i+m_start;\n" + " int batchIndex = i;\n" + " gConstraints[ srcIdx ].m_batchIdx = batchIndex; \n" + " }\n" + " }\n" + "}\n" + "#define CHECK_SIZE (WG_SIZE)\n" + "u32 readBuf(__local u32* buff, int idx)\n" + "{\n" + " idx = idx % (32*CHECK_SIZE);\n" + " int bitIdx = idx%32;\n" + " int bufIdx = idx/32;\n" + " return buff[bufIdx] & (1<<bitIdx);\n" + "}\n" + "void writeBuf(__local u32* buff, int idx)\n" + "{\n" + " idx = idx % (32*CHECK_SIZE);\n" + " int bitIdx = idx%32;\n" + " int bufIdx = idx/32;\n" + " buff[bufIdx] |= (1<<bitIdx);\n" + " //atom_or( &buff[bufIdx], (1<<bitIdx) );\n" + "}\n" + "u32 tryWrite(__local u32* buff, int idx)\n" + "{\n" + " idx = idx % (32*CHECK_SIZE);\n" + " int bitIdx = idx%32;\n" + " int bufIdx = idx/32;\n" + " u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) );\n" + " return ((ans >> bitIdx)&1) == 0;\n" + "}\n" + "// batching on the GPU\n" + "__kernel void CreateBatchesNew( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, __global int* batchSizes, int staticIdx )\n" + "{\n" + " int wgIdx = GET_GROUP_IDX;\n" + " int lIdx = GET_LOCAL_IDX;\n" + " const int numConstraints = gN[wgIdx];\n" + " const int m_start = gStart[wgIdx];\n" + " b3Contact4Data_t tmp;\n" + " \n" + " __local u32 ldsFixedBuffer[CHECK_SIZE];\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " if( lIdx == 0 )\n" + " {\n" + " \n" + " \n" + " __global struct b3Contact4Data* cs = &gConstraints[m_start]; \n" + " \n" + " \n" + " int numValidConstraints = 0;\n" + " int batchIdx = 0;\n" + " while( numValidConstraints < numConstraints)\n" + " {\n" + " int nCurrentBatch = 0;\n" + " // clear flag\n" + " \n" + " for(int i=0; i<CHECK_SIZE; i++) \n" + " ldsFixedBuffer[i] = 0; \n" + " for(int i=numValidConstraints; i<numConstraints; i++)\n" + " {\n" + " int bodyAS = cs[i].m_bodyAPtrAndSignBit;\n" + " int bodyBS = cs[i].m_bodyBPtrAndSignBit;\n" + " int bodyA = abs(bodyAS);\n" + " int bodyB = abs(bodyBS);\n" + " bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx;\n" + " bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx;\n" + " int aUnavailable = aIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyA);\n" + " int bUnavailable = bIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyB);\n" + " \n" + " if( aUnavailable==0 && bUnavailable==0 ) // ok\n" + " {\n" + " if (!aIsStatic)\n" + " {\n" + " writeBuf( ldsFixedBuffer, bodyA );\n" + " }\n" + " if (!bIsStatic)\n" + " {\n" + " writeBuf( ldsFixedBuffer, bodyB );\n" + " }\n" + " cs[i].m_batchIdx = batchIdx;\n" + " if (i!=numValidConstraints)\n" + " {\n" + " tmp = cs[i];\n" + " cs[i] = cs[numValidConstraints];\n" + " cs[numValidConstraints] = tmp;\n" + " }\n" + " numValidConstraints++;\n" + " \n" + " nCurrentBatch++;\n" + " if( nCurrentBatch == SIMD_WIDTH)\n" + " {\n" + " nCurrentBatch = 0;\n" + " for(int i=0; i<CHECK_SIZE; i++) \n" + " ldsFixedBuffer[i] = 0;\n" + " \n" + " }\n" + " }\n" + " }//for\n" + " batchIdx ++;\n" + " }//while\n" + " \n" + " batchSizes[wgIdx] = batchIdx;\n" + " }//if( lIdx == 0 )\n" + " \n" + " //return batchIdx;\n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h index a5a432947c..6e9c53e161 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h @@ -1,433 +1,432 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* integrateKernelCL= \ -"/*\n" -"Copyright (c) 2013 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Erwin Coumans\n" -"#ifndef B3_RIGIDBODY_DATA_H\n" -"#define B3_RIGIDBODY_DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#define B3_FLOAT4_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#define B3_PLATFORM_DEFINITIONS_H\n" -"struct MyTest\n" -"{\n" -" int bla;\n" -"};\n" -"#ifdef __cplusplus\n" -"#else\n" -"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" -"#define B3_LARGE_FLOAT 1e18f\n" -"#define B3_INFINITY 1e18f\n" -"#define b3Assert(a)\n" -"#define b3ConstArray(a) __global const a*\n" -"#define b3AtomicInc atomic_inc\n" -"#define b3AtomicAdd atomic_add\n" -"#define b3Fabs fabs\n" -"#define b3Sqrt native_sqrt\n" -"#define b3Sin native_sin\n" -"#define b3Cos native_cos\n" -"#define B3_STATIC\n" -"#endif\n" -"#endif\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Float4;\n" -" #define b3Float4ConstArg const b3Float4\n" -" #define b3MakeFloat4 (float4)\n" -" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return dot(a1, b1);\n" -" }\n" -" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return cross(a1, b1);\n" -" }\n" -" #define b3MinFloat4 min\n" -" #define b3MaxFloat4 max\n" -" #define b3Normalized(a) normalize(a)\n" -"#endif \n" -" \n" -"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" -"{\n" -" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" -" return false;\n" -" return true;\n" -"}\n" -"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" -"{\n" -" float maxDot = -B3_INFINITY;\n" -" int i = 0;\n" -" int ptIndex = -1;\n" -" for( i = 0; i < vecLen; i++ )\n" -" {\n" -" float dot = b3Dot3F4(vecArray[i],vec);\n" -" \n" -" if( dot > maxDot )\n" -" {\n" -" maxDot = dot;\n" -" ptIndex = i;\n" -" }\n" -" }\n" -" b3Assert(ptIndex>=0);\n" -" if (ptIndex<0)\n" -" {\n" -" ptIndex = 0;\n" -" }\n" -" *dotOut = maxDot;\n" -" return ptIndex;\n" -"}\n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_QUAT_H\n" -"#define B3_QUAT_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif\n" -"#endif\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Quat;\n" -" #define b3QuatConstArg const b3Quat\n" -" \n" -" \n" -"inline float4 b3FastNormalize4(float4 v)\n" -"{\n" -" v = (float4)(v.xyz,0.f);\n" -" return fast_normalize(v);\n" -"}\n" -" \n" -"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" -"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" -"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" -"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" -"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" -"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" -"{\n" -" b3Quat ans;\n" -" ans = b3Cross3( a, b );\n" -" ans += a.w*b+b.w*a;\n" -"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" -" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" -" return ans;\n" -"}\n" -"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" -"{\n" -" b3Quat q;\n" -" q=in;\n" -" //return b3FastNormalize4(in);\n" -" float len = native_sqrt(dot(q, q));\n" -" if(len > 0.f)\n" -" {\n" -" q *= 1.f / len;\n" -" }\n" -" else\n" -" {\n" -" q.x = q.y = q.z = 0.f;\n" -" q.w = 1.f;\n" -" }\n" -" return q;\n" -"}\n" -"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" -"{\n" -" b3Quat qInv = b3QuatInvert( q );\n" -" float4 vcpy = vec;\n" -" vcpy.w = 0.f;\n" -" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" -" return out;\n" -"}\n" -"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" -"{\n" -" return (b3Quat)(-q.xyz, q.w);\n" -"}\n" -"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" -"{\n" -" return (b3Quat)(-q.xyz, q.w);\n" -"}\n" -"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" -"{\n" -" return b3QuatRotate( b3QuatInvert( q ), vec );\n" -"}\n" -"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" -"{\n" -" return b3QuatRotate( orientation, point ) + (translation);\n" -"}\n" -" \n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"#ifndef B3_MAT3x3_H\n" -"#define B3_MAT3x3_H\n" -"#ifndef B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"typedef struct\n" -"{\n" -" b3Float4 m_row[3];\n" -"}b3Mat3x3;\n" -"#define b3Mat3x3ConstArg const b3Mat3x3\n" -"#define b3GetRow(m,row) (m.m_row[row])\n" -"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" -"{\n" -" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" -" b3Mat3x3 out;\n" -" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" -" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" -" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" -" out.m_row[0].w = 0.f;\n" -" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" -" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" -" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" -" out.m_row[1].w = 0.f;\n" -" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" -" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" -" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" -" out.m_row[2].w = 0.f;\n" -" return out;\n" -"}\n" -"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" -"{\n" -" b3Mat3x3 out;\n" -" out.m_row[0] = fabs(matIn.m_row[0]);\n" -" out.m_row[1] = fabs(matIn.m_row[1]);\n" -" out.m_row[2] = fabs(matIn.m_row[2]);\n" -" return out;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtZero();\n" -"__inline\n" -"b3Mat3x3 mtIdentity();\n" -"__inline\n" -"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" -"__inline\n" -"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" -"__inline\n" -"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" -"__inline\n" -"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" -"__inline\n" -"b3Mat3x3 mtZero()\n" -"{\n" -" b3Mat3x3 m;\n" -" m.m_row[0] = (b3Float4)(0.f);\n" -" m.m_row[1] = (b3Float4)(0.f);\n" -" m.m_row[2] = (b3Float4)(0.f);\n" -" return m;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtIdentity()\n" -"{\n" -" b3Mat3x3 m;\n" -" m.m_row[0] = (b3Float4)(1,0,0,0);\n" -" m.m_row[1] = (b3Float4)(0,1,0,0);\n" -" m.m_row[2] = (b3Float4)(0,0,1,0);\n" -" return m;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" -"{\n" -" b3Mat3x3 out;\n" -" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" -" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" -" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" -" return out;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" -"{\n" -" b3Mat3x3 transB;\n" -" transB = mtTranspose( b );\n" -" b3Mat3x3 ans;\n" -" // why this doesn't run when 0ing in the for{}\n" -" a.m_row[0].w = 0.f;\n" -" a.m_row[1].w = 0.f;\n" -" a.m_row[2].w = 0.f;\n" -" for(int i=0; i<3; i++)\n" -" {\n" -"// a.m_row[i].w = 0.f;\n" -" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" -" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" -" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" -" ans.m_row[i].w = 0.f;\n" -" }\n" -" return ans;\n" -"}\n" -"__inline\n" -"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" -"{\n" -" b3Float4 ans;\n" -" ans.x = b3Dot3F4( a.m_row[0], b );\n" -" ans.y = b3Dot3F4( a.m_row[1], b );\n" -" ans.z = b3Dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" -"{\n" -" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" b3Float4 ans;\n" -" ans.x = b3Dot3F4( a, colx );\n" -" ans.y = b3Dot3F4( a, coly );\n" -" ans.z = b3Dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"#endif\n" -"#endif //B3_MAT3x3_H\n" -"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" -"struct b3RigidBodyData\n" -"{\n" -" b3Float4 m_pos;\n" -" b3Quat m_quat;\n" -" b3Float4 m_linVel;\n" -" b3Float4 m_angVel;\n" -" int m_collidableIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"};\n" -"typedef struct b3InertiaData b3InertiaData_t;\n" -"struct b3InertiaData\n" -"{\n" -" b3Mat3x3 m_invInertiaWorld;\n" -" b3Mat3x3 m_initInvInertia;\n" -"};\n" -"#endif //B3_RIGIDBODY_DATA_H\n" -" \n" -"#ifndef B3_RIGIDBODY_DATA_H\n" -"#endif //B3_RIGIDBODY_DATA_H\n" -" \n" -"inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)\n" -"{\n" -" \n" -" if (bodies[nodeID].m_invMass != 0.f)\n" -" {\n" -" float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);\n" -" //angular velocity\n" -" {\n" -" b3Float4 axis;\n" -" //add some hardcoded angular damping\n" -" bodies[nodeID].m_angVel.x *= angularDamping;\n" -" bodies[nodeID].m_angVel.y *= angularDamping;\n" -" bodies[nodeID].m_angVel.z *= angularDamping;\n" -" \n" -" b3Float4 angvel = bodies[nodeID].m_angVel;\n" -" float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));\n" -" \n" -" //limit the angular motion\n" -" if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)\n" -" {\n" -" fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;\n" -" }\n" -" if(fAngle < 0.001f)\n" -" {\n" -" // use Taylor's expansions of sync function\n" -" axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);\n" -" }\n" -" else\n" -" {\n" -" // sync(fAngle) = sin(c*fAngle)/t\n" -" axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);\n" -" }\n" -" \n" -" b3Quat dorn;\n" -" dorn.x = axis.x;\n" -" dorn.y = axis.y;\n" -" dorn.z = axis.z;\n" -" dorn.w = b3Cos(fAngle * timeStep * 0.5f);\n" -" b3Quat orn0 = bodies[nodeID].m_quat;\n" -" b3Quat predictedOrn = b3QuatMul(dorn, orn0);\n" -" predictedOrn = b3QuatNormalized(predictedOrn);\n" -" bodies[nodeID].m_quat=predictedOrn;\n" -" }\n" -" //linear velocity \n" -" bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;\n" -" \n" -" //apply gravity\n" -" bodies[nodeID].m_linVel += gravityAcceleration * timeStep;\n" -" \n" -" }\n" -" \n" -"}\n" -"inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)\n" -"{\n" -" float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);\n" -" \n" -" if( (body->m_invMass != 0.f))\n" -" {\n" -" //angular velocity\n" -" {\n" -" b3Float4 axis;\n" -" //add some hardcoded angular damping\n" -" body->m_angVel.x *= angularDamping;\n" -" body->m_angVel.y *= angularDamping;\n" -" body->m_angVel.z *= angularDamping;\n" -" \n" -" b3Float4 angvel = body->m_angVel;\n" -" float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));\n" -" //limit the angular motion\n" -" if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)\n" -" {\n" -" fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;\n" -" }\n" -" if(fAngle < 0.001f)\n" -" {\n" -" // use Taylor's expansions of sync function\n" -" axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);\n" -" }\n" -" else\n" -" {\n" -" // sync(fAngle) = sin(c*fAngle)/t\n" -" axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);\n" -" }\n" -" b3Quat dorn;\n" -" dorn.x = axis.x;\n" -" dorn.y = axis.y;\n" -" dorn.z = axis.z;\n" -" dorn.w = b3Cos(fAngle * timeStep * 0.5f);\n" -" b3Quat orn0 = body->m_quat;\n" -" b3Quat predictedOrn = b3QuatMul(dorn, orn0);\n" -" predictedOrn = b3QuatNormalized(predictedOrn);\n" -" body->m_quat=predictedOrn;\n" -" }\n" -" //apply gravity\n" -" body->m_linVel += gravityAcceleration * timeStep;\n" -" //linear velocity \n" -" body->m_pos += body->m_linVel * timeStep;\n" -" \n" -" }\n" -" \n" -"}\n" -"__kernel void \n" -" integrateTransformsKernel( __global b3RigidBodyData_t* bodies,const int numNodes, float timeStep, float angularDamping, float4 gravityAcceleration)\n" -"{\n" -" int nodeID = get_global_id(0);\n" -" \n" -" if( nodeID < numNodes)\n" -" {\n" -" integrateSingleTransform(bodies,nodeID, timeStep, angularDamping,gravityAcceleration);\n" -" }\n" -"}\n" -; +static const char* integrateKernelCL = + "/*\n" + "Copyright (c) 2013 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Erwin Coumans\n" + "#ifndef B3_RIGIDBODY_DATA_H\n" + "#define B3_RIGIDBODY_DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#define B3_FLOAT4_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#define B3_PLATFORM_DEFINITIONS_H\n" + "struct MyTest\n" + "{\n" + " int bla;\n" + "};\n" + "#ifdef __cplusplus\n" + "#else\n" + "//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" + "#define B3_LARGE_FLOAT 1e18f\n" + "#define B3_INFINITY 1e18f\n" + "#define b3Assert(a)\n" + "#define b3ConstArray(a) __global const a*\n" + "#define b3AtomicInc atomic_inc\n" + "#define b3AtomicAdd atomic_add\n" + "#define b3Fabs fabs\n" + "#define b3Sqrt native_sqrt\n" + "#define b3Sin native_sin\n" + "#define b3Cos native_cos\n" + "#define B3_STATIC\n" + "#endif\n" + "#endif\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Float4;\n" + " #define b3Float4ConstArg const b3Float4\n" + " #define b3MakeFloat4 (float4)\n" + " float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return dot(a1, b1);\n" + " }\n" + " b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return cross(a1, b1);\n" + " }\n" + " #define b3MinFloat4 min\n" + " #define b3MaxFloat4 max\n" + " #define b3Normalized(a) normalize(a)\n" + "#endif \n" + " \n" + "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" + "{\n" + " if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" + " return false;\n" + " return true;\n" + "}\n" + "inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" + "{\n" + " float maxDot = -B3_INFINITY;\n" + " int i = 0;\n" + " int ptIndex = -1;\n" + " for( i = 0; i < vecLen; i++ )\n" + " {\n" + " float dot = b3Dot3F4(vecArray[i],vec);\n" + " \n" + " if( dot > maxDot )\n" + " {\n" + " maxDot = dot;\n" + " ptIndex = i;\n" + " }\n" + " }\n" + " b3Assert(ptIndex>=0);\n" + " if (ptIndex<0)\n" + " {\n" + " ptIndex = 0;\n" + " }\n" + " *dotOut = maxDot;\n" + " return ptIndex;\n" + "}\n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_QUAT_H\n" + "#define B3_QUAT_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif\n" + "#endif\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Quat;\n" + " #define b3QuatConstArg const b3Quat\n" + " \n" + " \n" + "inline float4 b3FastNormalize4(float4 v)\n" + "{\n" + " v = (float4)(v.xyz,0.f);\n" + " return fast_normalize(v);\n" + "}\n" + " \n" + "inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" + "inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" + "inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" + "inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" + "inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" + "inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" + "{\n" + " b3Quat ans;\n" + " ans = b3Cross3( a, b );\n" + " ans += a.w*b+b.w*a;\n" + "// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" + " ans.w = a.w*b.w - b3Dot3F4(a, b);\n" + " return ans;\n" + "}\n" + "inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" + "{\n" + " b3Quat q;\n" + " q=in;\n" + " //return b3FastNormalize4(in);\n" + " float len = native_sqrt(dot(q, q));\n" + " if(len > 0.f)\n" + " {\n" + " q *= 1.f / len;\n" + " }\n" + " else\n" + " {\n" + " q.x = q.y = q.z = 0.f;\n" + " q.w = 1.f;\n" + " }\n" + " return q;\n" + "}\n" + "inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" + "{\n" + " b3Quat qInv = b3QuatInvert( q );\n" + " float4 vcpy = vec;\n" + " vcpy.w = 0.f;\n" + " float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" + " return out;\n" + "}\n" + "inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" + "{\n" + " return (b3Quat)(-q.xyz, q.w);\n" + "}\n" + "inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" + "{\n" + " return (b3Quat)(-q.xyz, q.w);\n" + "}\n" + "inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" + "{\n" + " return b3QuatRotate( b3QuatInvert( q ), vec );\n" + "}\n" + "inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" + "{\n" + " return b3QuatRotate( orientation, point ) + (translation);\n" + "}\n" + " \n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "#ifndef B3_MAT3x3_H\n" + "#define B3_MAT3x3_H\n" + "#ifndef B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "typedef struct\n" + "{\n" + " b3Float4 m_row[3];\n" + "}b3Mat3x3;\n" + "#define b3Mat3x3ConstArg const b3Mat3x3\n" + "#define b3GetRow(m,row) (m.m_row[row])\n" + "inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" + "{\n" + " b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" + " b3Mat3x3 out;\n" + " out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" + " out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" + " out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" + " out.m_row[0].w = 0.f;\n" + " out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" + " out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" + " out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" + " out.m_row[1].w = 0.f;\n" + " out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" + " out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" + " out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" + " out.m_row[2].w = 0.f;\n" + " return out;\n" + "}\n" + "inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" + "{\n" + " b3Mat3x3 out;\n" + " out.m_row[0] = fabs(matIn.m_row[0]);\n" + " out.m_row[1] = fabs(matIn.m_row[1]);\n" + " out.m_row[2] = fabs(matIn.m_row[2]);\n" + " return out;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtZero();\n" + "__inline\n" + "b3Mat3x3 mtIdentity();\n" + "__inline\n" + "b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" + "__inline\n" + "b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" + "__inline\n" + "b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" + "__inline\n" + "b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" + "__inline\n" + "b3Mat3x3 mtZero()\n" + "{\n" + " b3Mat3x3 m;\n" + " m.m_row[0] = (b3Float4)(0.f);\n" + " m.m_row[1] = (b3Float4)(0.f);\n" + " m.m_row[2] = (b3Float4)(0.f);\n" + " return m;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtIdentity()\n" + "{\n" + " b3Mat3x3 m;\n" + " m.m_row[0] = (b3Float4)(1,0,0,0);\n" + " m.m_row[1] = (b3Float4)(0,1,0,0);\n" + " m.m_row[2] = (b3Float4)(0,0,1,0);\n" + " return m;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" + "{\n" + " b3Mat3x3 out;\n" + " out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" + " out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" + " out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" + " return out;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" + "{\n" + " b3Mat3x3 transB;\n" + " transB = mtTranspose( b );\n" + " b3Mat3x3 ans;\n" + " // why this doesn't run when 0ing in the for{}\n" + " a.m_row[0].w = 0.f;\n" + " a.m_row[1].w = 0.f;\n" + " a.m_row[2].w = 0.f;\n" + " for(int i=0; i<3; i++)\n" + " {\n" + "// a.m_row[i].w = 0.f;\n" + " ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" + " ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" + " ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" + " ans.m_row[i].w = 0.f;\n" + " }\n" + " return ans;\n" + "}\n" + "__inline\n" + "b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" + "{\n" + " b3Float4 ans;\n" + " ans.x = b3Dot3F4( a.m_row[0], b );\n" + " ans.y = b3Dot3F4( a.m_row[1], b );\n" + " ans.z = b3Dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" + "{\n" + " b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " b3Float4 ans;\n" + " ans.x = b3Dot3F4( a, colx );\n" + " ans.y = b3Dot3F4( a, coly );\n" + " ans.z = b3Dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "#endif\n" + "#endif //B3_MAT3x3_H\n" + "typedef struct b3RigidBodyData b3RigidBodyData_t;\n" + "struct b3RigidBodyData\n" + "{\n" + " b3Float4 m_pos;\n" + " b3Quat m_quat;\n" + " b3Float4 m_linVel;\n" + " b3Float4 m_angVel;\n" + " int m_collidableIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "};\n" + "typedef struct b3InertiaData b3InertiaData_t;\n" + "struct b3InertiaData\n" + "{\n" + " b3Mat3x3 m_invInertiaWorld;\n" + " b3Mat3x3 m_initInvInertia;\n" + "};\n" + "#endif //B3_RIGIDBODY_DATA_H\n" + " \n" + "#ifndef B3_RIGIDBODY_DATA_H\n" + "#endif //B3_RIGIDBODY_DATA_H\n" + " \n" + "inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)\n" + "{\n" + " \n" + " if (bodies[nodeID].m_invMass != 0.f)\n" + " {\n" + " float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);\n" + " //angular velocity\n" + " {\n" + " b3Float4 axis;\n" + " //add some hardcoded angular damping\n" + " bodies[nodeID].m_angVel.x *= angularDamping;\n" + " bodies[nodeID].m_angVel.y *= angularDamping;\n" + " bodies[nodeID].m_angVel.z *= angularDamping;\n" + " \n" + " b3Float4 angvel = bodies[nodeID].m_angVel;\n" + " float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));\n" + " \n" + " //limit the angular motion\n" + " if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)\n" + " {\n" + " fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;\n" + " }\n" + " if(fAngle < 0.001f)\n" + " {\n" + " // use Taylor's expansions of sync function\n" + " axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);\n" + " }\n" + " else\n" + " {\n" + " // sync(fAngle) = sin(c*fAngle)/t\n" + " axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);\n" + " }\n" + " \n" + " b3Quat dorn;\n" + " dorn.x = axis.x;\n" + " dorn.y = axis.y;\n" + " dorn.z = axis.z;\n" + " dorn.w = b3Cos(fAngle * timeStep * 0.5f);\n" + " b3Quat orn0 = bodies[nodeID].m_quat;\n" + " b3Quat predictedOrn = b3QuatMul(dorn, orn0);\n" + " predictedOrn = b3QuatNormalized(predictedOrn);\n" + " bodies[nodeID].m_quat=predictedOrn;\n" + " }\n" + " //linear velocity \n" + " bodies[nodeID].m_pos += bodies[nodeID].m_linVel * timeStep;\n" + " \n" + " //apply gravity\n" + " bodies[nodeID].m_linVel += gravityAcceleration * timeStep;\n" + " \n" + " }\n" + " \n" + "}\n" + "inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)\n" + "{\n" + " float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);\n" + " \n" + " if( (body->m_invMass != 0.f))\n" + " {\n" + " //angular velocity\n" + " {\n" + " b3Float4 axis;\n" + " //add some hardcoded angular damping\n" + " body->m_angVel.x *= angularDamping;\n" + " body->m_angVel.y *= angularDamping;\n" + " body->m_angVel.z *= angularDamping;\n" + " \n" + " b3Float4 angvel = body->m_angVel;\n" + " float fAngle = b3Sqrt(b3Dot3F4(angvel, angvel));\n" + " //limit the angular motion\n" + " if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)\n" + " {\n" + " fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;\n" + " }\n" + " if(fAngle < 0.001f)\n" + " {\n" + " // use Taylor's expansions of sync function\n" + " axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);\n" + " }\n" + " else\n" + " {\n" + " // sync(fAngle) = sin(c*fAngle)/t\n" + " axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);\n" + " }\n" + " b3Quat dorn;\n" + " dorn.x = axis.x;\n" + " dorn.y = axis.y;\n" + " dorn.z = axis.z;\n" + " dorn.w = b3Cos(fAngle * timeStep * 0.5f);\n" + " b3Quat orn0 = body->m_quat;\n" + " b3Quat predictedOrn = b3QuatMul(dorn, orn0);\n" + " predictedOrn = b3QuatNormalized(predictedOrn);\n" + " body->m_quat=predictedOrn;\n" + " }\n" + " //apply gravity\n" + " body->m_linVel += gravityAcceleration * timeStep;\n" + " //linear velocity \n" + " body->m_pos += body->m_linVel * timeStep;\n" + " \n" + " }\n" + " \n" + "}\n" + "__kernel void \n" + " integrateTransformsKernel( __global b3RigidBodyData_t* bodies,const int numNodes, float timeStep, float angularDamping, float4 gravityAcceleration)\n" + "{\n" + " int nodeID = get_global_id(0);\n" + " \n" + " if( nodeID < numNodes)\n" + " {\n" + " integrateSingleTransform(bodies,nodeID, timeStep, angularDamping,gravityAcceleration);\n" + " }\n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/jointSolver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/jointSolver.h index d48ecf6ea6..c94b55851e 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/jointSolver.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/jointSolver.h @@ -1,721 +1,720 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* solveConstraintRowsCL= \ -"/*\n" -"Copyright (c) 2013 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Erwin Coumans\n" -"#define B3_CONSTRAINT_FLAG_ENABLED 1\n" -"#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n" -"#define B3_GPU_FIXED_CONSTRAINT_TYPE 4\n" -"#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails\n" -"#define B3_INFINITY 1e30f\n" -"#define mymake_float4 (float4)\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" -"typedef float4 Quaternion;\n" -"typedef struct\n" -"{\n" -" float4 m_row[3];\n" -"}Matrix3x3;\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b);\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b);\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b)\n" -"{\n" -" float4 ans;\n" -" ans.x = dot3F4( a.m_row[0], b );\n" -" ans.y = dot3F4( a.m_row[1], b );\n" -" ans.z = dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b)\n" -"{\n" -" float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" float4 ans;\n" -" ans.x = dot3F4( a, colx );\n" -" ans.y = dot3F4( a, coly );\n" -" ans.z = dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"typedef struct\n" -"{\n" -" Matrix3x3 m_invInertiaWorld;\n" -" Matrix3x3 m_initInvInertia;\n" -"} BodyInertia;\n" -"typedef struct\n" -"{\n" -" Matrix3x3 m_basis;//orientation\n" -" float4 m_origin;//transform\n" -"}b3Transform;\n" -"typedef struct\n" -"{\n" -"// b3Transform m_worldTransformUnused;\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" -" union \n" -" {\n" -" void* m_originalBody;\n" -" int m_originalBodyIndex;\n" -" };\n" -" int padding[3];\n" -"} b3GpuSolverBody;\n" -"typedef struct\n" -"{\n" -" float4 m_pos;\n" -" Quaternion m_quat;\n" -" float4 m_linVel;\n" -" float4 m_angVel;\n" -" unsigned int m_shapeIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"} b3RigidBodyCL;\n" -"typedef struct\n" -"{\n" -" float4 m_relpos1CrossNormal;\n" -" float4 m_contactNormal;\n" -" float4 m_relpos2CrossNormal;\n" -" //float4 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal\n" -" float4 m_angularComponentA;\n" -" float4 m_angularComponentB;\n" -" \n" -" float m_appliedPushImpulse;\n" -" float m_appliedImpulse;\n" -" int m_padding1;\n" -" int m_padding2;\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" -" int m_originalConstraint;\n" -" int m_overrideNumSolverIterations;\n" -" int m_frictionIndex;\n" -" int m_solverBodyIdA;\n" -" int m_solverBodyIdB;\n" -"} b3SolverConstraint;\n" -"typedef struct \n" -"{\n" -" int m_bodyAPtrAndSignBit;\n" -" int m_bodyBPtrAndSignBit;\n" -" int m_originalConstraintIndex;\n" -" int m_batchId;\n" -"} b3BatchConstraint;\n" -"typedef struct \n" -"{\n" -" int m_constraintType;\n" -" int m_rbA;\n" -" int m_rbB;\n" -" float m_breakingImpulseThreshold;\n" -" float4 m_pivotInA;\n" -" float4 m_pivotInB;\n" -" Quaternion m_relTargetAB;\n" -" int m_flags;\n" -" int m_padding[3];\n" -"} b3GpuGenericConstraint;\n" -"/*b3Transform getWorldTransform(b3RigidBodyCL* rb)\n" -"{\n" -" b3Transform newTrans;\n" -" newTrans.setOrigin(rb->m_pos);\n" -" newTrans.setRotation(rb->m_quat);\n" -" return newTrans;\n" -"}*/\n" -"__inline\n" -"float4 cross3(float4 a, float4 b)\n" -"{\n" -" return cross(a,b);\n" -"}\n" -"__inline\n" -"float4 fastNormalize4(float4 v)\n" -"{\n" -" v = mymake_float4(v.xyz,0.f);\n" -" return fast_normalize(v);\n" -"}\n" -"__inline\n" -"Quaternion qtMul(Quaternion a, Quaternion b);\n" -"__inline\n" -"Quaternion qtNormalize(Quaternion in);\n" -"__inline\n" -"float4 qtRotate(Quaternion q, float4 vec);\n" -"__inline\n" -"Quaternion qtInvert(Quaternion q);\n" -"__inline\n" -"Quaternion qtMul(Quaternion a, Quaternion b)\n" -"{\n" -" Quaternion ans;\n" -" ans = cross3( a, b );\n" -" ans += a.w*b+b.w*a;\n" -"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" -" ans.w = a.w*b.w - dot3F4(a, b);\n" -" return ans;\n" -"}\n" -"__inline\n" -"Quaternion qtNormalize(Quaternion in)\n" -"{\n" -" return fastNormalize4(in);\n" -"// in /= length( in );\n" -"// return in;\n" -"}\n" -"__inline\n" -"float4 qtRotate(Quaternion q, float4 vec)\n" -"{\n" -" Quaternion qInv = qtInvert( q );\n" -" float4 vcpy = vec;\n" -" vcpy.w = 0.f;\n" -" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" -" return out;\n" -"}\n" -"__inline\n" -"Quaternion qtInvert(Quaternion q)\n" -"{\n" -" return (Quaternion)(-q.xyz, q.w);\n" -"}\n" -"__inline void internalApplyImpulse(__global b3GpuSolverBody* 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" -"void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c)\n" -"{\n" -" float deltaImpulse = c->m_rhs-c->m_appliedImpulse*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" -" deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv;\n" -" deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv;\n" -" float sum = c->m_appliedImpulse + deltaImpulse;\n" -" if (sum < c->m_lowerLimit)\n" -" {\n" -" deltaImpulse = c->m_lowerLimit-c->m_appliedImpulse;\n" -" c->m_appliedImpulse = c->m_lowerLimit;\n" -" }\n" -" else if (sum > c->m_upperLimit) \n" -" {\n" -" deltaImpulse = c->m_upperLimit-c->m_appliedImpulse;\n" -" c->m_appliedImpulse = c->m_upperLimit;\n" -" }\n" -" else\n" -" {\n" -" c->m_appliedImpulse = sum;\n" -" }\n" -" internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse);\n" -" internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse);\n" -"}\n" -"__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n" -" __global b3BatchConstraint* batchConstraints,\n" -" __global b3SolverConstraint* rows,\n" -" __global unsigned int* numConstraintRowsInfo1, \n" -" __global unsigned int* rowOffsets,\n" -" __global b3GpuGenericConstraint* constraints,\n" -" int batchOffset,\n" -" int numConstraintsInBatch\n" -" )\n" -"{\n" -" int b = get_global_id(0);\n" -" if (b>=numConstraintsInBatch)\n" -" return;\n" -" __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];\n" -" int originalConstraintIndex = c->m_originalConstraintIndex;\n" -" if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED)\n" -" {\n" -" int numConstraintRows = numConstraintRowsInfo1[originalConstraintIndex];\n" -" int rowOffset = rowOffsets[originalConstraintIndex];\n" -" for (int jj=0;jj<numConstraintRows;jj++)\n" -" {\n" -" __global b3SolverConstraint* constraint = &rows[rowOffset+jj];\n" -" resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);\n" -" }\n" -" }\n" -"};\n" -"__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies)\n" -"{\n" -" int i = get_global_id(0);\n" -" if (i>=numBodies)\n" -" return;\n" -" __global b3GpuSolverBody* solverBody = &solverBodies[i];\n" -" __global b3RigidBodyCL* bodyCL = &bodiesCL[i];\n" -" solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f);\n" -" solverBody->m_deltaAngularVelocity = (float4)(0.f,0.f,0.f,0.f);\n" -" solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n" -" solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n" -" solverBody->m_invMass = (float4)(bodyCL->m_invMass,bodyCL->m_invMass,bodyCL->m_invMass,0.f);\n" -" solverBody->m_originalBodyIndex = i;\n" -" solverBody->m_angularFactor = (float4)(1,1,1,0);\n" -" solverBody->m_linearFactor = (float4) (1,1,1,0);\n" -" solverBody->m_linearVelocity = bodyCL->m_linVel;\n" -" solverBody->m_angularVelocity = bodyCL->m_angVel;\n" -"}\n" -"__kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints)\n" -"{\n" -" int cid = get_global_id(0);\n" -" if (cid>=numConstraints)\n" -" return;\n" -" int numRows = numConstraintRows[cid];\n" -" if (numRows)\n" -" {\n" -" for (int i=0;i<numRows;i++)\n" -" {\n" -" int rowIndex = rowOffsets[cid]+i;\n" -" float breakingThreshold = constraints[cid].m_breakingImpulseThreshold;\n" -" if (fabs(rows[rowIndex].m_appliedImpulse) >= breakingThreshold)\n" -" {\n" -" constraints[cid].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;\n" -" }\n" -" }\n" -" }\n" -"}\n" -"__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints)\n" -"{\n" -" int i = get_global_id(0);\n" -" if (i>=numConstraints)\n" -" return;\n" -" __global b3GpuGenericConstraint* constraint = &constraints[i];\n" -" switch (constraint->m_constraintType)\n" -" {\n" -" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" -" {\n" -" infos[i] = 3;\n" -" break;\n" -" }\n" -" case B3_GPU_FIXED_CONSTRAINT_TYPE:\n" -" {\n" -" infos[i] = 6;\n" -" break;\n" -" }\n" -" default:\n" -" {\n" -" }\n" -" }\n" -"}\n" -"__kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, \n" -" __global b3BatchConstraint* batchConstraints, \n" -" __global b3GpuGenericConstraint* constraints,\n" -" __global b3RigidBodyCL* bodies,\n" -" int numConstraints)\n" -"{\n" -" int i = get_global_id(0);\n" -" if (i>=numConstraints)\n" -" return;\n" -" int rbA = constraints[i].m_rbA;\n" -" int rbB = constraints[i].m_rbB;\n" -" batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass != 0.f ? rbA : -rbA;\n" -" batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass != 0.f ? rbB : -rbB;\n" -" batchConstraints[i].m_batchId = -1;\n" -" batchConstraints[i].m_originalConstraintIndex = i;\n" -"}\n" -"typedef struct\n" -"{\n" -" // integrator parameters: frames per second (1/stepsize), default error\n" -" // reduction parameter (0..1).\n" -" float fps,erp;\n" -" // for the first and second body, pointers to two (linear and angular)\n" -" // n*3 jacobian sub matrices, stored by rows. these matrices will have\n" -" // been initialized to 0 on entry. if the second body is zero then the\n" -" // J2xx pointers may be 0.\n" -" union \n" -" {\n" -" __global float4* m_J1linearAxisFloat4;\n" -" __global float* m_J1linearAxis;\n" -" };\n" -" union\n" -" {\n" -" __global float4* m_J1angularAxisFloat4;\n" -" __global float* m_J1angularAxis;\n" -" };\n" -" union\n" -" {\n" -" __global float4* m_J2linearAxisFloat4;\n" -" __global float* m_J2linearAxis;\n" -" };\n" -" union\n" -" {\n" -" __global float4* m_J2angularAxisFloat4;\n" -" __global float* m_J2angularAxis;\n" -" };\n" -" // elements to jump from one row to the next in J's\n" -" int rowskip;\n" -" // right hand sides of the equation J*v = c + cfm * lambda. cfm is the\n" -" // \"constraint force mixing\" vector. c is set to zero on entry, cfm is\n" -" // set to a constant value (typically very small or zero) value on entry.\n" -" __global float* m_constraintError;\n" -" __global float* cfm;\n" -" // lo and hi limits for variables (set to -/+ infinity on entry).\n" -" __global float* m_lowerLimit;\n" -" __global float* m_upperLimit;\n" -" // findex vector for variables. see the LCP solver interface for a\n" -" // description of what this does. this is set to -1 on entry.\n" -" // note that the returned indexes are relative to the first index of\n" -" // the constraint.\n" -" __global int *findex;\n" -" // number of solver iterations\n" -" int m_numIterations;\n" -" //damping of the velocity\n" -" float m_damping;\n" -"} b3GpuConstraintInfo2;\n" -"void getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2)\n" -"{\n" -" *v0 = (float4)(0. ,-vecIn.z ,vecIn.y,0.f);\n" -" *v1 = (float4)(vecIn.z ,0. ,-vecIn.x,0.f);\n" -" *v2 = (float4)(-vecIn.y ,vecIn.x ,0.f,0.f);\n" -"}\n" -"void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies)\n" -"{\n" -" float4 posA = bodies[constraint->m_rbA].m_pos;\n" -" Quaternion rotA = bodies[constraint->m_rbA].m_quat;\n" -" float4 posB = bodies[constraint->m_rbB].m_pos;\n" -" Quaternion rotB = bodies[constraint->m_rbB].m_quat;\n" -" // anchor points in global coordinates with respect to body PORs.\n" -" \n" -" // set jacobian\n" -" info->m_J1linearAxis[0] = 1;\n" -" info->m_J1linearAxis[info->rowskip+1] = 1;\n" -" info->m_J1linearAxis[2*info->rowskip+2] = 1;\n" -" float4 a1 = qtRotate(rotA,constraint->m_pivotInA);\n" -" {\n" -" __global float4* angular0 = (__global float4*)(info->m_J1angularAxis);\n" -" __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip);\n" -" __global float4* angular2 = (__global float4*)(info->m_J1angularAxis+2*info->rowskip);\n" -" float4 a1neg = -a1;\n" -" getSkewSymmetricMatrix(a1neg,angular0,angular1,angular2);\n" -" }\n" -" if (info->m_J2linearAxis)\n" -" {\n" -" info->m_J2linearAxis[0] = -1;\n" -" info->m_J2linearAxis[info->rowskip+1] = -1;\n" -" info->m_J2linearAxis[2*info->rowskip+2] = -1;\n" -" }\n" -" \n" -" float4 a2 = qtRotate(rotB,constraint->m_pivotInB);\n" -" \n" -" {\n" -" // float4 a2n = -a2;\n" -" __global float4* angular0 = (__global float4*)(info->m_J2angularAxis);\n" -" __global float4* angular1 = (__global float4*)(info->m_J2angularAxis+info->rowskip);\n" -" __global float4* angular2 = (__global float4*)(info->m_J2angularAxis+2*info->rowskip);\n" -" getSkewSymmetricMatrix(a2,angular0,angular1,angular2);\n" -" }\n" -" \n" -" // set right hand side\n" -"// float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;\n" -" float currERP = info->erp;\n" -" float k = info->fps * currERP;\n" -" int j;\n" -" float4 result = a2 + posB - a1 - posA;\n" -" float* resultPtr = &result;\n" -" for (j=0; j<3; j++)\n" -" {\n" -" info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]);\n" -" }\n" -"}\n" -"Quaternion nearest( Quaternion first, Quaternion qd)\n" -"{\n" -" Quaternion diff,sum;\n" -" diff = first- qd;\n" -" sum = first + qd;\n" -" \n" -" if( dot(diff,diff) < dot(sum,sum) )\n" -" return qd;\n" -" return (-qd);\n" -"}\n" -"float b3Acos(float x) \n" -"{ \n" -" if (x<-1) \n" -" x=-1; \n" -" if (x>1) \n" -" x=1;\n" -" return acos(x); \n" -"}\n" -"float getAngle(Quaternion orn)\n" -"{\n" -" if (orn.w>=1.f)\n" -" orn.w=1.f;\n" -" float s = 2.f * b3Acos(orn.w);\n" -" return s;\n" -"}\n" -"void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle)\n" -"{\n" -" Quaternion orn1 = nearest(orn0,orn1a);\n" -" \n" -" Quaternion dorn = qtMul(orn1,qtInvert(orn0));\n" -" *angle = getAngle(dorn);\n" -" *axis = (float4)(dorn.x,dorn.y,dorn.z,0.f);\n" -" \n" -" //check for axis length\n" -" float len = dot3F4(*axis,*axis);\n" -" if (len < FLT_EPSILON*FLT_EPSILON)\n" -" *axis = (float4)(1,0,0,0);\n" -" else\n" -" *axis /= sqrt(len);\n" -"}\n" -"void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)\n" -"{\n" -" Quaternion worldOrnA = bodies[constraint->m_rbA].m_quat;\n" -" Quaternion worldOrnB = bodies[constraint->m_rbB].m_quat;\n" -" int s = info->rowskip;\n" -" int start_index = start_row * s;\n" -" // 3 rows to make body rotations equal\n" -" info->m_J1angularAxis[start_index] = 1;\n" -" info->m_J1angularAxis[start_index + s + 1] = 1;\n" -" info->m_J1angularAxis[start_index + s*2+2] = 1;\n" -" if ( info->m_J2angularAxis)\n" -" {\n" -" info->m_J2angularAxis[start_index] = -1;\n" -" info->m_J2angularAxis[start_index + s+1] = -1;\n" -" info->m_J2angularAxis[start_index + s*2+2] = -1;\n" -" }\n" -" \n" -" float currERP = info->erp;\n" -" float k = info->fps * currERP;\n" -" float4 diff;\n" -" float angle;\n" -" float4 qrelCur = qtMul(worldOrnA,qtInvert(worldOrnB));\n" -" \n" -" calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,&diff,&angle);\n" -" diff*=-angle;\n" -" \n" -" float* resultPtr = &diff;\n" -" \n" -" for (int j=0; j<3; j++)\n" -" {\n" -" info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j];\n" -" }\n" -" \n" -"}\n" -"__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies)\n" -"{\n" -" int i = get_global_id(0);\n" -" if (i>=numBodies)\n" -" return;\n" -" if (bodies[i].m_invMass)\n" -" {\n" -"// if (length(solverBodies[i].m_deltaLinearVelocity)<MOTIONCLAMP)\n" -" {\n" -" bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;\n" -" }\n" -"// if (length(solverBodies[i].m_deltaAngularVelocity)<MOTIONCLAMP)\n" -" {\n" -" bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;\n" -" } \n" -" }\n" -"}\n" -"__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, \n" -" __global unsigned int* infos, \n" -" __global unsigned int* constraintRowOffsets, \n" -" __global b3GpuGenericConstraint* constraints, \n" -" __global b3BatchConstraint* batchConstraints, \n" -" __global b3RigidBodyCL* bodies,\n" -" __global BodyInertia* inertias,\n" -" __global b3GpuSolverBody* solverBodies,\n" -" float timeStep,\n" -" float globalErp,\n" -" float globalCfm,\n" -" float globalDamping,\n" -" int globalNumIterations,\n" -" int numConstraints)\n" -"{\n" -" int i = get_global_id(0);\n" -" if (i>=numConstraints)\n" -" return;\n" -" \n" -" //for now, always initialize the batch info\n" -" int info1 = infos[i];\n" -" \n" -" __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]];\n" -" __global b3GpuGenericConstraint* constraint = &constraints[i];\n" -" __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n" -" __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n" -" int solverBodyIdA = constraint->m_rbA;\n" -" int solverBodyIdB = constraint->m_rbB;\n" -" __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n" -" __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n" -" if (rbA->m_invMass)\n" -" {\n" -" batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;\n" -" } else\n" -" {\n" -"// if (!solverBodyIdA)\n" -"// m_staticIdx = 0;\n" -" batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;\n" -" }\n" -" if (rbB->m_invMass)\n" -" {\n" -" batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;\n" -" } else\n" -" {\n" -"// if (!solverBodyIdB)\n" -"// m_staticIdx = 0;\n" -" batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;\n" -" }\n" -" if (info1)\n" -" {\n" -" int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n" -"// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n" -" // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n" -" int j;\n" -" for ( j=0;j<info1;j++)\n" -" {\n" -"// memset(¤tConstraintRow[j],0,sizeof(b3SolverConstraint));\n" -" currentConstraintRow[j].m_angularComponentA = (float4)(0,0,0,0);\n" -" currentConstraintRow[j].m_angularComponentB = (float4)(0,0,0,0);\n" -" currentConstraintRow[j].m_appliedImpulse = 0.f;\n" -" currentConstraintRow[j].m_appliedPushImpulse = 0.f;\n" -" currentConstraintRow[j].m_cfm = 0.f;\n" -" currentConstraintRow[j].m_contactNormal = (float4)(0,0,0,0);\n" -" currentConstraintRow[j].m_friction = 0.f;\n" -" currentConstraintRow[j].m_frictionIndex = 0;\n" -" currentConstraintRow[j].m_jacDiagABInv = 0.f;\n" -" currentConstraintRow[j].m_lowerLimit = 0.f;\n" -" currentConstraintRow[j].m_upperLimit = 0.f;\n" -" currentConstraintRow[j].m_originalConstraint = i;\n" -" currentConstraintRow[j].m_overrideNumSolverIterations = 0;\n" -" currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0);\n" -" currentConstraintRow[j].m_relpos2CrossNormal = (float4)(0,0,0,0);\n" -" currentConstraintRow[j].m_rhs = 0.f;\n" -" currentConstraintRow[j].m_rhsPenetration = 0.f;\n" -" currentConstraintRow[j].m_solverBodyIdA = 0;\n" -" currentConstraintRow[j].m_solverBodyIdB = 0;\n" -" \n" -" currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;\n" -" currentConstraintRow[j].m_upperLimit = B3_INFINITY;\n" -" currentConstraintRow[j].m_appliedImpulse = 0.f;\n" -" currentConstraintRow[j].m_appliedPushImpulse = 0.f;\n" -" currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;\n" -" currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;\n" -" currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; \n" -" }\n" -" bodyAPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n" -" bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n" -" bodyAPtr->m_pushVelocity = (float4)(0,0,0,0);\n" -" bodyAPtr->m_turnVelocity = (float4)(0,0,0,0);\n" -" bodyBPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n" -" bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n" -" bodyBPtr->m_pushVelocity = (float4)(0,0,0,0);\n" -" bodyBPtr->m_turnVelocity = (float4)(0,0,0,0);\n" -" int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n" -" \n" -" b3GpuConstraintInfo2 info2;\n" -" info2.fps = 1.f/timeStep;\n" -" info2.erp = globalErp;\n" -" info2.m_J1linearAxisFloat4 = ¤tConstraintRow->m_contactNormal;\n" -" info2.m_J1angularAxisFloat4 = ¤tConstraintRow->m_relpos1CrossNormal;\n" -" info2.m_J2linearAxisFloat4 = 0;\n" -" info2.m_J2angularAxisFloat4 = ¤tConstraintRow->m_relpos2CrossNormal;\n" -" info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n" -" ///the size of b3SolverConstraint needs be a multiple of float\n" -"// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint));\n" -" info2.m_constraintError = ¤tConstraintRow->m_rhs;\n" -" currentConstraintRow->m_cfm = globalCfm;\n" -" info2.m_damping = globalDamping;\n" -" info2.cfm = ¤tConstraintRow->m_cfm;\n" -" info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;\n" -" info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;\n" -" info2.m_numIterations = globalNumIterations;\n" -" switch (constraint->m_constraintType)\n" -" {\n" -" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" -" {\n" -" getInfo2Point2Point(constraint,&info2,bodies);\n" -" break;\n" -" }\n" -" case B3_GPU_FIXED_CONSTRAINT_TYPE:\n" -" {\n" -" getInfo2Point2Point(constraint,&info2,bodies);\n" -" getInfo2FixedOrientation(constraint,&info2,bodies,3);\n" -" break;\n" -" }\n" -" default:\n" -" {\n" -" }\n" -" }\n" -" ///finalize the constraint setup\n" -" for ( j=0;j<info1;j++)\n" -" {\n" -" __global b3SolverConstraint* solverConstraint = ¤tConstraintRow[j];\n" -" if (solverConstraint->m_upperLimit>=constraint->m_breakingImpulseThreshold)\n" -" {\n" -" solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold;\n" -" }\n" -" if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold)\n" -" {\n" -" solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold;\n" -" }\n" -"// solverConstraint->m_originalContactPoint = constraint;\n" -" \n" -" Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld;\n" -" {\n" -" //float4 angularFactorA(1,1,1);\n" -" float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal;\n" -" solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA;\n" -" }\n" -" \n" -" Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld;\n" -" {\n" -" float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal;\n" -" solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor();\n" -" }\n" -" {\n" -" //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal\n" -" //because it gets multiplied iMJlB\n" -" float4 iMJlA = solverConstraint->m_contactNormal*rbA->m_invMass;\n" -" float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA);\n" -" float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal?\n" -" float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB);\n" -" float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal);\n" -" sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal);\n" -" sum += dot3F4(iMJlB,solverConstraint->m_contactNormal);\n" -" sum += dot3F4(iMJaB,solverConstraint->m_relpos2CrossNormal);\n" -" float fsum = fabs(sum);\n" -" if (fsum>FLT_EPSILON)\n" -" {\n" -" solverConstraint->m_jacDiagABInv = 1.f/sum;\n" -" } else\n" -" {\n" -" solverConstraint->m_jacDiagABInv = 0.f;\n" -" }\n" -" }\n" -" ///fix rhs\n" -" ///todo: add force/torque accelerators\n" -" {\n" -" float rel_vel;\n" -" float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel);\n" -" float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel);\n" -" rel_vel = vel1Dotn+vel2Dotn;\n" -" float restitution = 0.f;\n" -" float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2\n" -" float velocityError = restitution - rel_vel * info2.m_damping;\n" -" float penetrationImpulse = positionalError*solverConstraint->m_jacDiagABInv;\n" -" float velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv;\n" -" solverConstraint->m_rhs = penetrationImpulse+velocityImpulse;\n" -" solverConstraint->m_appliedImpulse = 0.f;\n" -" }\n" -" }\n" -" }\n" -"}\n" -; +static const char* solveConstraintRowsCL = + "/*\n" + "Copyright (c) 2013 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Erwin Coumans\n" + "#define B3_CONSTRAINT_FLAG_ENABLED 1\n" + "#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n" + "#define B3_GPU_FIXED_CONSTRAINT_TYPE 4\n" + "#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails\n" + "#define B3_INFINITY 1e30f\n" + "#define mymake_float4 (float4)\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" + "typedef float4 Quaternion;\n" + "typedef struct\n" + "{\n" + " float4 m_row[3];\n" + "}Matrix3x3;\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b);\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b);\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b)\n" + "{\n" + " float4 ans;\n" + " ans.x = dot3F4( a.m_row[0], b );\n" + " ans.y = dot3F4( a.m_row[1], b );\n" + " ans.z = dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b)\n" + "{\n" + " float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " float4 ans;\n" + " ans.x = dot3F4( a, colx );\n" + " ans.y = dot3F4( a, coly );\n" + " ans.z = dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "typedef struct\n" + "{\n" + " Matrix3x3 m_invInertiaWorld;\n" + " Matrix3x3 m_initInvInertia;\n" + "} BodyInertia;\n" + "typedef struct\n" + "{\n" + " Matrix3x3 m_basis;//orientation\n" + " float4 m_origin;//transform\n" + "}b3Transform;\n" + "typedef struct\n" + "{\n" + "// b3Transform m_worldTransformUnused;\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" + " union \n" + " {\n" + " void* m_originalBody;\n" + " int m_originalBodyIndex;\n" + " };\n" + " int padding[3];\n" + "} b3GpuSolverBody;\n" + "typedef struct\n" + "{\n" + " float4 m_pos;\n" + " Quaternion m_quat;\n" + " float4 m_linVel;\n" + " float4 m_angVel;\n" + " unsigned int m_shapeIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "} b3RigidBodyCL;\n" + "typedef struct\n" + "{\n" + " float4 m_relpos1CrossNormal;\n" + " float4 m_contactNormal;\n" + " float4 m_relpos2CrossNormal;\n" + " //float4 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal\n" + " float4 m_angularComponentA;\n" + " float4 m_angularComponentB;\n" + " \n" + " float m_appliedPushImpulse;\n" + " float m_appliedImpulse;\n" + " int m_padding1;\n" + " int m_padding2;\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" + " int m_originalConstraint;\n" + " int m_overrideNumSolverIterations;\n" + " int m_frictionIndex;\n" + " int m_solverBodyIdA;\n" + " int m_solverBodyIdB;\n" + "} b3SolverConstraint;\n" + "typedef struct \n" + "{\n" + " int m_bodyAPtrAndSignBit;\n" + " int m_bodyBPtrAndSignBit;\n" + " int m_originalConstraintIndex;\n" + " int m_batchId;\n" + "} b3BatchConstraint;\n" + "typedef struct \n" + "{\n" + " int m_constraintType;\n" + " int m_rbA;\n" + " int m_rbB;\n" + " float m_breakingImpulseThreshold;\n" + " float4 m_pivotInA;\n" + " float4 m_pivotInB;\n" + " Quaternion m_relTargetAB;\n" + " int m_flags;\n" + " int m_padding[3];\n" + "} b3GpuGenericConstraint;\n" + "/*b3Transform getWorldTransform(b3RigidBodyCL* rb)\n" + "{\n" + " b3Transform newTrans;\n" + " newTrans.setOrigin(rb->m_pos);\n" + " newTrans.setRotation(rb->m_quat);\n" + " return newTrans;\n" + "}*/\n" + "__inline\n" + "float4 cross3(float4 a, float4 b)\n" + "{\n" + " return cross(a,b);\n" + "}\n" + "__inline\n" + "float4 fastNormalize4(float4 v)\n" + "{\n" + " v = mymake_float4(v.xyz,0.f);\n" + " return fast_normalize(v);\n" + "}\n" + "__inline\n" + "Quaternion qtMul(Quaternion a, Quaternion b);\n" + "__inline\n" + "Quaternion qtNormalize(Quaternion in);\n" + "__inline\n" + "float4 qtRotate(Quaternion q, float4 vec);\n" + "__inline\n" + "Quaternion qtInvert(Quaternion q);\n" + "__inline\n" + "Quaternion qtMul(Quaternion a, Quaternion b)\n" + "{\n" + " Quaternion ans;\n" + " ans = cross3( a, b );\n" + " ans += a.w*b+b.w*a;\n" + "// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" + " ans.w = a.w*b.w - dot3F4(a, b);\n" + " return ans;\n" + "}\n" + "__inline\n" + "Quaternion qtNormalize(Quaternion in)\n" + "{\n" + " return fastNormalize4(in);\n" + "// in /= length( in );\n" + "// return in;\n" + "}\n" + "__inline\n" + "float4 qtRotate(Quaternion q, float4 vec)\n" + "{\n" + " Quaternion qInv = qtInvert( q );\n" + " float4 vcpy = vec;\n" + " vcpy.w = 0.f;\n" + " float4 out = qtMul(qtMul(q,vcpy),qInv);\n" + " return out;\n" + "}\n" + "__inline\n" + "Quaternion qtInvert(Quaternion q)\n" + "{\n" + " return (Quaternion)(-q.xyz, q.w);\n" + "}\n" + "__inline void internalApplyImpulse(__global b3GpuSolverBody* 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" + "void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c)\n" + "{\n" + " float deltaImpulse = c->m_rhs-c->m_appliedImpulse*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" + " deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv;\n" + " deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv;\n" + " float sum = c->m_appliedImpulse + deltaImpulse;\n" + " if (sum < c->m_lowerLimit)\n" + " {\n" + " deltaImpulse = c->m_lowerLimit-c->m_appliedImpulse;\n" + " c->m_appliedImpulse = c->m_lowerLimit;\n" + " }\n" + " else if (sum > c->m_upperLimit) \n" + " {\n" + " deltaImpulse = c->m_upperLimit-c->m_appliedImpulse;\n" + " c->m_appliedImpulse = c->m_upperLimit;\n" + " }\n" + " else\n" + " {\n" + " c->m_appliedImpulse = sum;\n" + " }\n" + " internalApplyImpulse(body1,c->m_contactNormal*body1->m_invMass,c->m_angularComponentA,deltaImpulse);\n" + " internalApplyImpulse(body2,-c->m_contactNormal*body2->m_invMass,c->m_angularComponentB,deltaImpulse);\n" + "}\n" + "__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n" + " __global b3BatchConstraint* batchConstraints,\n" + " __global b3SolverConstraint* rows,\n" + " __global unsigned int* numConstraintRowsInfo1, \n" + " __global unsigned int* rowOffsets,\n" + " __global b3GpuGenericConstraint* constraints,\n" + " int batchOffset,\n" + " int numConstraintsInBatch\n" + " )\n" + "{\n" + " int b = get_global_id(0);\n" + " if (b>=numConstraintsInBatch)\n" + " return;\n" + " __global b3BatchConstraint* c = &batchConstraints[b+batchOffset];\n" + " int originalConstraintIndex = c->m_originalConstraintIndex;\n" + " if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED)\n" + " {\n" + " int numConstraintRows = numConstraintRowsInfo1[originalConstraintIndex];\n" + " int rowOffset = rowOffsets[originalConstraintIndex];\n" + " for (int jj=0;jj<numConstraintRows;jj++)\n" + " {\n" + " __global b3SolverConstraint* constraint = &rows[rowOffset+jj];\n" + " resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint);\n" + " }\n" + " }\n" + "};\n" + "__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies)\n" + "{\n" + " int i = get_global_id(0);\n" + " if (i>=numBodies)\n" + " return;\n" + " __global b3GpuSolverBody* solverBody = &solverBodies[i];\n" + " __global b3RigidBodyCL* bodyCL = &bodiesCL[i];\n" + " solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f);\n" + " solverBody->m_deltaAngularVelocity = (float4)(0.f,0.f,0.f,0.f);\n" + " solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n" + " solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f);\n" + " solverBody->m_invMass = (float4)(bodyCL->m_invMass,bodyCL->m_invMass,bodyCL->m_invMass,0.f);\n" + " solverBody->m_originalBodyIndex = i;\n" + " solverBody->m_angularFactor = (float4)(1,1,1,0);\n" + " solverBody->m_linearFactor = (float4) (1,1,1,0);\n" + " solverBody->m_linearVelocity = bodyCL->m_linVel;\n" + " solverBody->m_angularVelocity = bodyCL->m_angVel;\n" + "}\n" + "__kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints)\n" + "{\n" + " int cid = get_global_id(0);\n" + " if (cid>=numConstraints)\n" + " return;\n" + " int numRows = numConstraintRows[cid];\n" + " if (numRows)\n" + " {\n" + " for (int i=0;i<numRows;i++)\n" + " {\n" + " int rowIndex = rowOffsets[cid]+i;\n" + " float breakingThreshold = constraints[cid].m_breakingImpulseThreshold;\n" + " if (fabs(rows[rowIndex].m_appliedImpulse) >= breakingThreshold)\n" + " {\n" + " constraints[cid].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED;\n" + " }\n" + " }\n" + " }\n" + "}\n" + "__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints)\n" + "{\n" + " int i = get_global_id(0);\n" + " if (i>=numConstraints)\n" + " return;\n" + " __global b3GpuGenericConstraint* constraint = &constraints[i];\n" + " switch (constraint->m_constraintType)\n" + " {\n" + " case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" + " {\n" + " infos[i] = 3;\n" + " break;\n" + " }\n" + " case B3_GPU_FIXED_CONSTRAINT_TYPE:\n" + " {\n" + " infos[i] = 6;\n" + " break;\n" + " }\n" + " default:\n" + " {\n" + " }\n" + " }\n" + "}\n" + "__kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, \n" + " __global b3BatchConstraint* batchConstraints, \n" + " __global b3GpuGenericConstraint* constraints,\n" + " __global b3RigidBodyCL* bodies,\n" + " int numConstraints)\n" + "{\n" + " int i = get_global_id(0);\n" + " if (i>=numConstraints)\n" + " return;\n" + " int rbA = constraints[i].m_rbA;\n" + " int rbB = constraints[i].m_rbB;\n" + " batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass != 0.f ? rbA : -rbA;\n" + " batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass != 0.f ? rbB : -rbB;\n" + " batchConstraints[i].m_batchId = -1;\n" + " batchConstraints[i].m_originalConstraintIndex = i;\n" + "}\n" + "typedef struct\n" + "{\n" + " // integrator parameters: frames per second (1/stepsize), default error\n" + " // reduction parameter (0..1).\n" + " float fps,erp;\n" + " // for the first and second body, pointers to two (linear and angular)\n" + " // n*3 jacobian sub matrices, stored by rows. these matrices will have\n" + " // been initialized to 0 on entry. if the second body is zero then the\n" + " // J2xx pointers may be 0.\n" + " union \n" + " {\n" + " __global float4* m_J1linearAxisFloat4;\n" + " __global float* m_J1linearAxis;\n" + " };\n" + " union\n" + " {\n" + " __global float4* m_J1angularAxisFloat4;\n" + " __global float* m_J1angularAxis;\n" + " };\n" + " union\n" + " {\n" + " __global float4* m_J2linearAxisFloat4;\n" + " __global float* m_J2linearAxis;\n" + " };\n" + " union\n" + " {\n" + " __global float4* m_J2angularAxisFloat4;\n" + " __global float* m_J2angularAxis;\n" + " };\n" + " // elements to jump from one row to the next in J's\n" + " int rowskip;\n" + " // right hand sides of the equation J*v = c + cfm * lambda. cfm is the\n" + " // \"constraint force mixing\" vector. c is set to zero on entry, cfm is\n" + " // set to a constant value (typically very small or zero) value on entry.\n" + " __global float* m_constraintError;\n" + " __global float* cfm;\n" + " // lo and hi limits for variables (set to -/+ infinity on entry).\n" + " __global float* m_lowerLimit;\n" + " __global float* m_upperLimit;\n" + " // findex vector for variables. see the LCP solver interface for a\n" + " // description of what this does. this is set to -1 on entry.\n" + " // note that the returned indexes are relative to the first index of\n" + " // the constraint.\n" + " __global int *findex;\n" + " // number of solver iterations\n" + " int m_numIterations;\n" + " //damping of the velocity\n" + " float m_damping;\n" + "} b3GpuConstraintInfo2;\n" + "void getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2)\n" + "{\n" + " *v0 = (float4)(0. ,-vecIn.z ,vecIn.y,0.f);\n" + " *v1 = (float4)(vecIn.z ,0. ,-vecIn.x,0.f);\n" + " *v2 = (float4)(-vecIn.y ,vecIn.x ,0.f,0.f);\n" + "}\n" + "void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies)\n" + "{\n" + " float4 posA = bodies[constraint->m_rbA].m_pos;\n" + " Quaternion rotA = bodies[constraint->m_rbA].m_quat;\n" + " float4 posB = bodies[constraint->m_rbB].m_pos;\n" + " Quaternion rotB = bodies[constraint->m_rbB].m_quat;\n" + " // anchor points in global coordinates with respect to body PORs.\n" + " \n" + " // set jacobian\n" + " info->m_J1linearAxis[0] = 1;\n" + " info->m_J1linearAxis[info->rowskip+1] = 1;\n" + " info->m_J1linearAxis[2*info->rowskip+2] = 1;\n" + " float4 a1 = qtRotate(rotA,constraint->m_pivotInA);\n" + " {\n" + " __global float4* angular0 = (__global float4*)(info->m_J1angularAxis);\n" + " __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip);\n" + " __global float4* angular2 = (__global float4*)(info->m_J1angularAxis+2*info->rowskip);\n" + " float4 a1neg = -a1;\n" + " getSkewSymmetricMatrix(a1neg,angular0,angular1,angular2);\n" + " }\n" + " if (info->m_J2linearAxis)\n" + " {\n" + " info->m_J2linearAxis[0] = -1;\n" + " info->m_J2linearAxis[info->rowskip+1] = -1;\n" + " info->m_J2linearAxis[2*info->rowskip+2] = -1;\n" + " }\n" + " \n" + " float4 a2 = qtRotate(rotB,constraint->m_pivotInB);\n" + " \n" + " {\n" + " // float4 a2n = -a2;\n" + " __global float4* angular0 = (__global float4*)(info->m_J2angularAxis);\n" + " __global float4* angular1 = (__global float4*)(info->m_J2angularAxis+info->rowskip);\n" + " __global float4* angular2 = (__global float4*)(info->m_J2angularAxis+2*info->rowskip);\n" + " getSkewSymmetricMatrix(a2,angular0,angular1,angular2);\n" + " }\n" + " \n" + " // set right hand side\n" + "// float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;\n" + " float currERP = info->erp;\n" + " float k = info->fps * currERP;\n" + " int j;\n" + " float4 result = a2 + posB - a1 - posA;\n" + " float* resultPtr = &result;\n" + " for (j=0; j<3; j++)\n" + " {\n" + " info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]);\n" + " }\n" + "}\n" + "Quaternion nearest( Quaternion first, Quaternion qd)\n" + "{\n" + " Quaternion diff,sum;\n" + " diff = first- qd;\n" + " sum = first + qd;\n" + " \n" + " if( dot(diff,diff) < dot(sum,sum) )\n" + " return qd;\n" + " return (-qd);\n" + "}\n" + "float b3Acos(float x) \n" + "{ \n" + " if (x<-1) \n" + " x=-1; \n" + " if (x>1) \n" + " x=1;\n" + " return acos(x); \n" + "}\n" + "float getAngle(Quaternion orn)\n" + "{\n" + " if (orn.w>=1.f)\n" + " orn.w=1.f;\n" + " float s = 2.f * b3Acos(orn.w);\n" + " return s;\n" + "}\n" + "void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle)\n" + "{\n" + " Quaternion orn1 = nearest(orn0,orn1a);\n" + " \n" + " Quaternion dorn = qtMul(orn1,qtInvert(orn0));\n" + " *angle = getAngle(dorn);\n" + " *axis = (float4)(dorn.x,dorn.y,dorn.z,0.f);\n" + " \n" + " //check for axis length\n" + " float len = dot3F4(*axis,*axis);\n" + " if (len < FLT_EPSILON*FLT_EPSILON)\n" + " *axis = (float4)(1,0,0,0);\n" + " else\n" + " *axis /= sqrt(len);\n" + "}\n" + "void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row)\n" + "{\n" + " Quaternion worldOrnA = bodies[constraint->m_rbA].m_quat;\n" + " Quaternion worldOrnB = bodies[constraint->m_rbB].m_quat;\n" + " int s = info->rowskip;\n" + " int start_index = start_row * s;\n" + " // 3 rows to make body rotations equal\n" + " info->m_J1angularAxis[start_index] = 1;\n" + " info->m_J1angularAxis[start_index + s + 1] = 1;\n" + " info->m_J1angularAxis[start_index + s*2+2] = 1;\n" + " if ( info->m_J2angularAxis)\n" + " {\n" + " info->m_J2angularAxis[start_index] = -1;\n" + " info->m_J2angularAxis[start_index + s+1] = -1;\n" + " info->m_J2angularAxis[start_index + s*2+2] = -1;\n" + " }\n" + " \n" + " float currERP = info->erp;\n" + " float k = info->fps * currERP;\n" + " float4 diff;\n" + " float angle;\n" + " float4 qrelCur = qtMul(worldOrnA,qtInvert(worldOrnB));\n" + " \n" + " calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,&diff,&angle);\n" + " diff*=-angle;\n" + " \n" + " float* resultPtr = &diff;\n" + " \n" + " for (int j=0; j<3; j++)\n" + " {\n" + " info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j];\n" + " }\n" + " \n" + "}\n" + "__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies)\n" + "{\n" + " int i = get_global_id(0);\n" + " if (i>=numBodies)\n" + " return;\n" + " if (bodies[i].m_invMass)\n" + " {\n" + "// if (length(solverBodies[i].m_deltaLinearVelocity)<MOTIONCLAMP)\n" + " {\n" + " bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;\n" + " }\n" + "// if (length(solverBodies[i].m_deltaAngularVelocity)<MOTIONCLAMP)\n" + " {\n" + " bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;\n" + " } \n" + " }\n" + "}\n" + "__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, \n" + " __global unsigned int* infos, \n" + " __global unsigned int* constraintRowOffsets, \n" + " __global b3GpuGenericConstraint* constraints, \n" + " __global b3BatchConstraint* batchConstraints, \n" + " __global b3RigidBodyCL* bodies,\n" + " __global BodyInertia* inertias,\n" + " __global b3GpuSolverBody* solverBodies,\n" + " float timeStep,\n" + " float globalErp,\n" + " float globalCfm,\n" + " float globalDamping,\n" + " int globalNumIterations,\n" + " int numConstraints)\n" + "{\n" + " int i = get_global_id(0);\n" + " if (i>=numConstraints)\n" + " return;\n" + " \n" + " //for now, always initialize the batch info\n" + " int info1 = infos[i];\n" + " \n" + " __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]];\n" + " __global b3GpuGenericConstraint* constraint = &constraints[i];\n" + " __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n" + " __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n" + " int solverBodyIdA = constraint->m_rbA;\n" + " int solverBodyIdB = constraint->m_rbB;\n" + " __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n" + " __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n" + " if (rbA->m_invMass)\n" + " {\n" + " batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;\n" + " } else\n" + " {\n" + "// if (!solverBodyIdA)\n" + "// m_staticIdx = 0;\n" + " batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;\n" + " }\n" + " if (rbB->m_invMass)\n" + " {\n" + " batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;\n" + " } else\n" + " {\n" + "// if (!solverBodyIdB)\n" + "// m_staticIdx = 0;\n" + " batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;\n" + " }\n" + " if (info1)\n" + " {\n" + " int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n" + "// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n" + " // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n" + " int j;\n" + " for ( j=0;j<info1;j++)\n" + " {\n" + "// memset(¤tConstraintRow[j],0,sizeof(b3SolverConstraint));\n" + " currentConstraintRow[j].m_angularComponentA = (float4)(0,0,0,0);\n" + " currentConstraintRow[j].m_angularComponentB = (float4)(0,0,0,0);\n" + " currentConstraintRow[j].m_appliedImpulse = 0.f;\n" + " currentConstraintRow[j].m_appliedPushImpulse = 0.f;\n" + " currentConstraintRow[j].m_cfm = 0.f;\n" + " currentConstraintRow[j].m_contactNormal = (float4)(0,0,0,0);\n" + " currentConstraintRow[j].m_friction = 0.f;\n" + " currentConstraintRow[j].m_frictionIndex = 0;\n" + " currentConstraintRow[j].m_jacDiagABInv = 0.f;\n" + " currentConstraintRow[j].m_lowerLimit = 0.f;\n" + " currentConstraintRow[j].m_upperLimit = 0.f;\n" + " currentConstraintRow[j].m_originalConstraint = i;\n" + " currentConstraintRow[j].m_overrideNumSolverIterations = 0;\n" + " currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0);\n" + " currentConstraintRow[j].m_relpos2CrossNormal = (float4)(0,0,0,0);\n" + " currentConstraintRow[j].m_rhs = 0.f;\n" + " currentConstraintRow[j].m_rhsPenetration = 0.f;\n" + " currentConstraintRow[j].m_solverBodyIdA = 0;\n" + " currentConstraintRow[j].m_solverBodyIdB = 0;\n" + " \n" + " currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;\n" + " currentConstraintRow[j].m_upperLimit = B3_INFINITY;\n" + " currentConstraintRow[j].m_appliedImpulse = 0.f;\n" + " currentConstraintRow[j].m_appliedPushImpulse = 0.f;\n" + " currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;\n" + " currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;\n" + " currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; \n" + " }\n" + " bodyAPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n" + " bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n" + " bodyAPtr->m_pushVelocity = (float4)(0,0,0,0);\n" + " bodyAPtr->m_turnVelocity = (float4)(0,0,0,0);\n" + " bodyBPtr->m_deltaLinearVelocity = (float4)(0,0,0,0);\n" + " bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0);\n" + " bodyBPtr->m_pushVelocity = (float4)(0,0,0,0);\n" + " bodyBPtr->m_turnVelocity = (float4)(0,0,0,0);\n" + " int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n" + " \n" + " b3GpuConstraintInfo2 info2;\n" + " info2.fps = 1.f/timeStep;\n" + " info2.erp = globalErp;\n" + " info2.m_J1linearAxisFloat4 = ¤tConstraintRow->m_contactNormal;\n" + " info2.m_J1angularAxisFloat4 = ¤tConstraintRow->m_relpos1CrossNormal;\n" + " info2.m_J2linearAxisFloat4 = 0;\n" + " info2.m_J2angularAxisFloat4 = ¤tConstraintRow->m_relpos2CrossNormal;\n" + " info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n" + " ///the size of b3SolverConstraint needs be a multiple of float\n" + "// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint));\n" + " info2.m_constraintError = ¤tConstraintRow->m_rhs;\n" + " currentConstraintRow->m_cfm = globalCfm;\n" + " info2.m_damping = globalDamping;\n" + " info2.cfm = ¤tConstraintRow->m_cfm;\n" + " info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;\n" + " info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;\n" + " info2.m_numIterations = globalNumIterations;\n" + " switch (constraint->m_constraintType)\n" + " {\n" + " case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n" + " {\n" + " getInfo2Point2Point(constraint,&info2,bodies);\n" + " break;\n" + " }\n" + " case B3_GPU_FIXED_CONSTRAINT_TYPE:\n" + " {\n" + " getInfo2Point2Point(constraint,&info2,bodies);\n" + " getInfo2FixedOrientation(constraint,&info2,bodies,3);\n" + " break;\n" + " }\n" + " default:\n" + " {\n" + " }\n" + " }\n" + " ///finalize the constraint setup\n" + " for ( j=0;j<info1;j++)\n" + " {\n" + " __global b3SolverConstraint* solverConstraint = ¤tConstraintRow[j];\n" + " if (solverConstraint->m_upperLimit>=constraint->m_breakingImpulseThreshold)\n" + " {\n" + " solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold;\n" + " }\n" + " if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold)\n" + " {\n" + " solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold;\n" + " }\n" + "// solverConstraint->m_originalContactPoint = constraint;\n" + " \n" + " Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld;\n" + " {\n" + " //float4 angularFactorA(1,1,1);\n" + " float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal;\n" + " solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA;\n" + " }\n" + " \n" + " Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld;\n" + " {\n" + " float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal;\n" + " solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor();\n" + " }\n" + " {\n" + " //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal\n" + " //because it gets multiplied iMJlB\n" + " float4 iMJlA = solverConstraint->m_contactNormal*rbA->m_invMass;\n" + " float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA);\n" + " float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal?\n" + " float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB);\n" + " float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal);\n" + " sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal);\n" + " sum += dot3F4(iMJlB,solverConstraint->m_contactNormal);\n" + " sum += dot3F4(iMJaB,solverConstraint->m_relpos2CrossNormal);\n" + " float fsum = fabs(sum);\n" + " if (fsum>FLT_EPSILON)\n" + " {\n" + " solverConstraint->m_jacDiagABInv = 1.f/sum;\n" + " } else\n" + " {\n" + " solverConstraint->m_jacDiagABInv = 0.f;\n" + " }\n" + " }\n" + " ///fix rhs\n" + " ///todo: add force/torque accelerators\n" + " {\n" + " float rel_vel;\n" + " float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel);\n" + " float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel);\n" + " rel_vel = vel1Dotn+vel2Dotn;\n" + " float restitution = 0.f;\n" + " float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2\n" + " float velocityError = restitution - rel_vel * info2.m_damping;\n" + " float penetrationImpulse = positionalError*solverConstraint->m_jacDiagABInv;\n" + " float velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv;\n" + " solverConstraint->m_rhs = penetrationImpulse+velocityImpulse;\n" + " solverConstraint->m_appliedImpulse = 0.f;\n" + " }\n" + " }\n" + " }\n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveContact.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveContact.h index 15a049992b..6e14ad51fc 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveContact.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveContact.h @@ -1,393 +1,392 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* solveContactCL= \ -"/*\n" -"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Takahiro Harada\n" -"//#pragma OPENCL EXTENSION cl_amd_printf : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" -"#ifdef cl_ext_atomic_counters_32\n" -"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" -"#else\n" -"#define counter32_t volatile global int*\n" -"#endif\n" -"typedef unsigned int u32;\n" -"typedef unsigned short u16;\n" -"typedef unsigned char u8;\n" -"#define GET_GROUP_IDX get_group_id(0)\n" -"#define GET_LOCAL_IDX get_local_id(0)\n" -"#define GET_GLOBAL_IDX get_global_id(0)\n" -"#define GET_GROUP_SIZE get_local_size(0)\n" -"#define GET_NUM_GROUPS get_num_groups(0)\n" -"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" -"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" -"#define AtomInc(x) atom_inc(&(x))\n" -"#define AtomInc1(x, out) out = atom_inc(&(x))\n" -"#define AppendInc(x, out) out = atomic_inc(x)\n" -"#define AtomAdd(x, value) atom_add(&(x), value)\n" -"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" -"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" -"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" -"#define mymake_float4 (float4)\n" -"//#define make_float2 (float2)\n" -"//#define make_uint4 (uint4)\n" -"//#define make_int4 (int4)\n" -"//#define make_uint2 (uint2)\n" -"//#define make_int2 (int2)\n" -"#define max2 max\n" -"#define min2 min\n" -"///////////////////////////////////////\n" -"// Vector\n" -"///////////////////////////////////////\n" -"__inline\n" -"float4 fastNormalize4(float4 v)\n" -"{\n" -" return fast_normalize(v);\n" -"}\n" -"__inline\n" -"float4 cross3(float4 a, float4 b)\n" -"{\n" -" return cross(a,b);\n" -"}\n" -"__inline\n" -"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" -"__inline\n" -"float4 normalize3(const float4 a)\n" -"{\n" -" float4 n = mymake_float4(a.x, a.y, a.z, 0.f);\n" -" return fastNormalize4( n );\n" -"// float length = sqrtf(dot3F4(a, a));\n" -"// return 1.f/length * a;\n" -"}\n" -"///////////////////////////////////////\n" -"// Matrix3x3\n" -"///////////////////////////////////////\n" -"typedef struct\n" -"{\n" -" float4 m_row[3];\n" -"}Matrix3x3;\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b);\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b);\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b)\n" -"{\n" -" float4 ans;\n" -" ans.x = dot3F4( a.m_row[0], b );\n" -" ans.y = dot3F4( a.m_row[1], b );\n" -" ans.z = dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b)\n" -"{\n" -" float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" float4 ans;\n" -" ans.x = dot3F4( a, colx );\n" -" ans.y = dot3F4( a, coly );\n" -" ans.z = dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"///////////////////////////////////////\n" -"// Quaternion\n" -"///////////////////////////////////////\n" -"typedef float4 Quaternion;\n" -"#define WG_SIZE 64\n" -"typedef struct\n" -"{\n" -" float4 m_pos;\n" -" Quaternion m_quat;\n" -" float4 m_linVel;\n" -" float4 m_angVel;\n" -" u32 m_shapeIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"} Body;\n" -"typedef struct\n" -"{\n" -" Matrix3x3 m_invInertia;\n" -" Matrix3x3 m_initInvInertia;\n" -"} Shape;\n" -"typedef struct\n" -"{\n" -" float4 m_linear;\n" -" float4 m_worldPos[4];\n" -" float4 m_center; \n" -" float m_jacCoeffInv[4];\n" -" float m_b[4];\n" -" float m_appliedRambdaDt[4];\n" -" float m_fJacCoeffInv[2]; \n" -" float m_fAppliedRambdaDt[2]; \n" -" u32 m_bodyA;\n" -" u32 m_bodyB;\n" -" int m_batchIdx;\n" -" u32 m_paddings[1];\n" -"} Constraint4;\n" -"typedef struct\n" -"{\n" -" int m_nConstraints;\n" -" int m_start;\n" -" int m_batchIdx;\n" -" int m_nSplit;\n" -"// int m_paddings[1];\n" -"} ConstBuffer;\n" -"typedef struct\n" -"{\n" -" int m_solveFriction;\n" -" int m_maxBatch; // long batch really kills the performance\n" -" int m_batchIdx;\n" -" int m_nSplit;\n" -"// int m_paddings[1];\n" -"} ConstBufferBatchSolve;\n" -"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1);\n" -"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n" -"{\n" -" *linear = mymake_float4(-n.xyz,0.f);\n" -" *angular0 = -cross3(r0, n);\n" -" *angular1 = cross3(r1, n);\n" -"}\n" -"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 );\n" -"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n" -"{\n" -" return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n" -"}\n" -"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" -" float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1);\n" -"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" -" float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)\n" -"{\n" -" // linear0,1 are normlized\n" -" float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n" -" float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" -" float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n" -" float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" -" return -1.f/(jmj0+jmj1+jmj2+jmj3);\n" -"}\n" -"void solveContact(__global Constraint4* cs,\n" -" float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n" -" float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB);\n" -"void solveContact(__global Constraint4* cs,\n" -" float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n" -" float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB)\n" -"{\n" -" float minRambdaDt = 0;\n" -" float maxRambdaDt = FLT_MAX;\n" -" for(int ic=0; ic<4; ic++)\n" -" {\n" -" if( cs->m_jacCoeffInv[ic] == 0.f ) continue;\n" -" float4 angular0, angular1, linear;\n" -" float4 r0 = cs->m_worldPos[ic] - posA;\n" -" float4 r1 = cs->m_worldPos[ic] - posB;\n" -" setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n" -" float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n" -" *linVelA, *angVelA, *linVelB, *angVelB ) + cs->m_b[ic];\n" -" rambdaDt *= cs->m_jacCoeffInv[ic];\n" -" {\n" -" float prevSum = cs->m_appliedRambdaDt[ic];\n" -" float updated = prevSum;\n" -" updated += rambdaDt;\n" -" updated = max2( updated, minRambdaDt );\n" -" updated = min2( updated, maxRambdaDt );\n" -" rambdaDt = updated - prevSum;\n" -" cs->m_appliedRambdaDt[ic] = updated;\n" -" }\n" -" float4 linImp0 = invMassA*linear*rambdaDt;\n" -" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" -" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" -" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" -" *linVelA += linImp0;\n" -" *angVelA += angImp0;\n" -" *linVelB += linImp1;\n" -" *angVelB += angImp1;\n" -" }\n" -"}\n" -"void btPlaneSpace1 (const float4* n, float4* p, float4* q);\n" -" void btPlaneSpace1 (const float4* n, float4* p, float4* q)\n" -"{\n" -" if (fabs(n[0].z) > 0.70710678f) {\n" -" // choose p in y-z plane\n" -" float a = n[0].y*n[0].y + n[0].z*n[0].z;\n" -" float k = 1.f/sqrt(a);\n" -" p[0].x = 0;\n" -" p[0].y = -n[0].z*k;\n" -" p[0].z = n[0].y*k;\n" -" // set q = n x p\n" -" q[0].x = a*k;\n" -" q[0].y = -n[0].x*p[0].z;\n" -" q[0].z = n[0].x*p[0].y;\n" -" }\n" -" else {\n" -" // choose p in x-y plane\n" -" float a = n[0].x*n[0].x + n[0].y*n[0].y;\n" -" float k = 1.f/sqrt(a);\n" -" p[0].x = -n[0].y*k;\n" -" p[0].y = n[0].x*k;\n" -" p[0].z = 0;\n" -" // set q = n x p\n" -" q[0].x = -n[0].z*p[0].y;\n" -" q[0].y = n[0].z*p[0].x;\n" -" q[0].z = a*k;\n" -" }\n" -"}\n" -"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);\n" -"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)\n" -"{\n" -" //float frictionCoeff = ldsCs[0].m_linear.w;\n" -" int aIdx = ldsCs[0].m_bodyA;\n" -" int bIdx = ldsCs[0].m_bodyB;\n" -" float4 posA = gBodies[aIdx].m_pos;\n" -" float4 linVelA = gBodies[aIdx].m_linVel;\n" -" float4 angVelA = gBodies[aIdx].m_angVel;\n" -" float invMassA = gBodies[aIdx].m_invMass;\n" -" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" -" float4 posB = gBodies[bIdx].m_pos;\n" -" float4 linVelB = gBodies[bIdx].m_linVel;\n" -" float4 angVelB = gBodies[bIdx].m_angVel;\n" -" float invMassB = gBodies[bIdx].m_invMass;\n" -" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" -" solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" -" posB, &linVelB, &angVelB, invMassB, invInertiaB );\n" -" if (gBodies[aIdx].m_invMass)\n" -" {\n" -" gBodies[aIdx].m_linVel = linVelA;\n" -" gBodies[aIdx].m_angVel = angVelA;\n" -" } else\n" -" {\n" -" gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);\n" -" gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);\n" -" \n" -" }\n" -" if (gBodies[bIdx].m_invMass)\n" -" {\n" -" gBodies[bIdx].m_linVel = linVelB;\n" -" gBodies[bIdx].m_angVel = angVelB;\n" -" } else\n" -" {\n" -" gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);\n" -" gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);\n" -" \n" -" }\n" -"}\n" -"typedef struct \n" -"{\n" -" int m_valInt0;\n" -" int m_valInt1;\n" -" int m_valInt2;\n" -" int m_valInt3;\n" -" float m_val0;\n" -" float m_val1;\n" -" float m_val2;\n" -" float m_val3;\n" -"} SolverDebugInfo;\n" -"__kernel\n" -"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void BatchSolveKernelContact(__global Body* gBodies,\n" -" __global Shape* gShapes,\n" -" __global Constraint4* gConstraints,\n" -" __global int* gN,\n" -" __global int* gOffsets,\n" -" __global int* batchSizes,\n" -" int maxBatch1,\n" -" int cellBatch,\n" -" int4 nSplit\n" -" )\n" -"{\n" -" //__local int ldsBatchIdx[WG_SIZE+1];\n" -" __local int ldsCurBatch;\n" -" __local int ldsNextBatch;\n" -" __local int ldsStart;\n" -" int lIdx = GET_LOCAL_IDX;\n" -" int wgIdx = GET_GROUP_IDX;\n" -"// int gIdx = GET_GLOBAL_IDX;\n" -"// debugInfo[gIdx].m_valInt0 = gIdx;\n" -" //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;\n" -" \n" -" \n" -" int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2);\n" -" int remain= (wgIdx%((nSplit.x*nSplit.y)/4));\n" -" int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1);\n" -" int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1);\n" -" int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y);\n" -" //int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1);\n" -" //int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1);\n" -" //int cellIdx = xIdx+yIdx*nSplit;\n" -" \n" -" if( gN[cellIdx] == 0 ) \n" -" return;\n" -" int maxBatch = batchSizes[cellIdx];\n" -" \n" -" \n" -" const int start = gOffsets[cellIdx];\n" -" const int end = start + gN[cellIdx];\n" -" \n" -" \n" -" \n" -" if( lIdx == 0 )\n" -" {\n" -" ldsCurBatch = 0;\n" -" ldsNextBatch = 0;\n" -" ldsStart = start;\n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" int idx=ldsStart+lIdx;\n" -" while (ldsCurBatch < maxBatch)\n" -" {\n" -" for(; idx<end; )\n" -" {\n" -" if (gConstraints[idx].m_batchIdx == ldsCurBatch)\n" -" {\n" -" solveContactConstraint( gBodies, gShapes, &gConstraints[idx] );\n" -" idx+=64;\n" -" } else\n" -" {\n" -" break;\n" -" }\n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" \n" -" if( lIdx == 0 )\n" -" {\n" -" ldsCurBatch++;\n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" }\n" -" \n" -" \n" -"}\n" -"__kernel void solveSingleContactKernel(__global Body* gBodies,\n" -" __global Shape* gShapes,\n" -" __global Constraint4* gConstraints,\n" -" int cellIdx,\n" -" int batchOffset,\n" -" int numConstraintsInBatch\n" -" )\n" -"{\n" -" int index = get_global_id(0);\n" -" if (index < numConstraintsInBatch)\n" -" {\n" -" int idx=batchOffset+index;\n" -" solveContactConstraint( gBodies, gShapes, &gConstraints[idx] );\n" -" } \n" -"}\n" -; +static const char* solveContactCL = + "/*\n" + "Copyright (c) 2012 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Takahiro Harada\n" + "//#pragma OPENCL EXTENSION cl_amd_printf : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" + "#ifdef cl_ext_atomic_counters_32\n" + "#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" + "#else\n" + "#define counter32_t volatile global int*\n" + "#endif\n" + "typedef unsigned int u32;\n" + "typedef unsigned short u16;\n" + "typedef unsigned char u8;\n" + "#define GET_GROUP_IDX get_group_id(0)\n" + "#define GET_LOCAL_IDX get_local_id(0)\n" + "#define GET_GLOBAL_IDX get_global_id(0)\n" + "#define GET_GROUP_SIZE get_local_size(0)\n" + "#define GET_NUM_GROUPS get_num_groups(0)\n" + "#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" + "#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" + "#define AtomInc(x) atom_inc(&(x))\n" + "#define AtomInc1(x, out) out = atom_inc(&(x))\n" + "#define AppendInc(x, out) out = atomic_inc(x)\n" + "#define AtomAdd(x, value) atom_add(&(x), value)\n" + "#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" + "#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" + "#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" + "#define mymake_float4 (float4)\n" + "//#define make_float2 (float2)\n" + "//#define make_uint4 (uint4)\n" + "//#define make_int4 (int4)\n" + "//#define make_uint2 (uint2)\n" + "//#define make_int2 (int2)\n" + "#define max2 max\n" + "#define min2 min\n" + "///////////////////////////////////////\n" + "// Vector\n" + "///////////////////////////////////////\n" + "__inline\n" + "float4 fastNormalize4(float4 v)\n" + "{\n" + " return fast_normalize(v);\n" + "}\n" + "__inline\n" + "float4 cross3(float4 a, float4 b)\n" + "{\n" + " return cross(a,b);\n" + "}\n" + "__inline\n" + "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" + "__inline\n" + "float4 normalize3(const float4 a)\n" + "{\n" + " float4 n = mymake_float4(a.x, a.y, a.z, 0.f);\n" + " return fastNormalize4( n );\n" + "// float length = sqrtf(dot3F4(a, a));\n" + "// return 1.f/length * a;\n" + "}\n" + "///////////////////////////////////////\n" + "// Matrix3x3\n" + "///////////////////////////////////////\n" + "typedef struct\n" + "{\n" + " float4 m_row[3];\n" + "}Matrix3x3;\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b);\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b);\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b)\n" + "{\n" + " float4 ans;\n" + " ans.x = dot3F4( a.m_row[0], b );\n" + " ans.y = dot3F4( a.m_row[1], b );\n" + " ans.z = dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b)\n" + "{\n" + " float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " float4 ans;\n" + " ans.x = dot3F4( a, colx );\n" + " ans.y = dot3F4( a, coly );\n" + " ans.z = dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "///////////////////////////////////////\n" + "// Quaternion\n" + "///////////////////////////////////////\n" + "typedef float4 Quaternion;\n" + "#define WG_SIZE 64\n" + "typedef struct\n" + "{\n" + " float4 m_pos;\n" + " Quaternion m_quat;\n" + " float4 m_linVel;\n" + " float4 m_angVel;\n" + " u32 m_shapeIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "} Body;\n" + "typedef struct\n" + "{\n" + " Matrix3x3 m_invInertia;\n" + " Matrix3x3 m_initInvInertia;\n" + "} Shape;\n" + "typedef struct\n" + "{\n" + " float4 m_linear;\n" + " float4 m_worldPos[4];\n" + " float4 m_center; \n" + " float m_jacCoeffInv[4];\n" + " float m_b[4];\n" + " float m_appliedRambdaDt[4];\n" + " float m_fJacCoeffInv[2]; \n" + " float m_fAppliedRambdaDt[2]; \n" + " u32 m_bodyA;\n" + " u32 m_bodyB;\n" + " int m_batchIdx;\n" + " u32 m_paddings[1];\n" + "} Constraint4;\n" + "typedef struct\n" + "{\n" + " int m_nConstraints;\n" + " int m_start;\n" + " int m_batchIdx;\n" + " int m_nSplit;\n" + "// int m_paddings[1];\n" + "} ConstBuffer;\n" + "typedef struct\n" + "{\n" + " int m_solveFriction;\n" + " int m_maxBatch; // long batch really kills the performance\n" + " int m_batchIdx;\n" + " int m_nSplit;\n" + "// int m_paddings[1];\n" + "} ConstBufferBatchSolve;\n" + "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1);\n" + "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n" + "{\n" + " *linear = mymake_float4(-n.xyz,0.f);\n" + " *angular0 = -cross3(r0, n);\n" + " *angular1 = cross3(r1, n);\n" + "}\n" + "float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 );\n" + "float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n" + "{\n" + " return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n" + "}\n" + "float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" + " float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1);\n" + "float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" + " float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)\n" + "{\n" + " // linear0,1 are normlized\n" + " float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n" + " float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" + " float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n" + " float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" + " return -1.f/(jmj0+jmj1+jmj2+jmj3);\n" + "}\n" + "void solveContact(__global Constraint4* cs,\n" + " float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n" + " float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB);\n" + "void solveContact(__global Constraint4* cs,\n" + " float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n" + " float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB)\n" + "{\n" + " float minRambdaDt = 0;\n" + " float maxRambdaDt = FLT_MAX;\n" + " for(int ic=0; ic<4; ic++)\n" + " {\n" + " if( cs->m_jacCoeffInv[ic] == 0.f ) continue;\n" + " float4 angular0, angular1, linear;\n" + " float4 r0 = cs->m_worldPos[ic] - posA;\n" + " float4 r1 = cs->m_worldPos[ic] - posB;\n" + " setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n" + " float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n" + " *linVelA, *angVelA, *linVelB, *angVelB ) + cs->m_b[ic];\n" + " rambdaDt *= cs->m_jacCoeffInv[ic];\n" + " {\n" + " float prevSum = cs->m_appliedRambdaDt[ic];\n" + " float updated = prevSum;\n" + " updated += rambdaDt;\n" + " updated = max2( updated, minRambdaDt );\n" + " updated = min2( updated, maxRambdaDt );\n" + " rambdaDt = updated - prevSum;\n" + " cs->m_appliedRambdaDt[ic] = updated;\n" + " }\n" + " float4 linImp0 = invMassA*linear*rambdaDt;\n" + " float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" + " float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" + " float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" + " *linVelA += linImp0;\n" + " *angVelA += angImp0;\n" + " *linVelB += linImp1;\n" + " *angVelB += angImp1;\n" + " }\n" + "}\n" + "void btPlaneSpace1 (const float4* n, float4* p, float4* q);\n" + " void btPlaneSpace1 (const float4* n, float4* p, float4* q)\n" + "{\n" + " if (fabs(n[0].z) > 0.70710678f) {\n" + " // choose p in y-z plane\n" + " float a = n[0].y*n[0].y + n[0].z*n[0].z;\n" + " float k = 1.f/sqrt(a);\n" + " p[0].x = 0;\n" + " p[0].y = -n[0].z*k;\n" + " p[0].z = n[0].y*k;\n" + " // set q = n x p\n" + " q[0].x = a*k;\n" + " q[0].y = -n[0].x*p[0].z;\n" + " q[0].z = n[0].x*p[0].y;\n" + " }\n" + " else {\n" + " // choose p in x-y plane\n" + " float a = n[0].x*n[0].x + n[0].y*n[0].y;\n" + " float k = 1.f/sqrt(a);\n" + " p[0].x = -n[0].y*k;\n" + " p[0].y = n[0].x*k;\n" + " p[0].z = 0;\n" + " // set q = n x p\n" + " q[0].x = -n[0].z*p[0].y;\n" + " q[0].y = n[0].z*p[0].x;\n" + " q[0].z = a*k;\n" + " }\n" + "}\n" + "void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);\n" + "void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)\n" + "{\n" + " //float frictionCoeff = ldsCs[0].m_linear.w;\n" + " int aIdx = ldsCs[0].m_bodyA;\n" + " int bIdx = ldsCs[0].m_bodyB;\n" + " float4 posA = gBodies[aIdx].m_pos;\n" + " float4 linVelA = gBodies[aIdx].m_linVel;\n" + " float4 angVelA = gBodies[aIdx].m_angVel;\n" + " float invMassA = gBodies[aIdx].m_invMass;\n" + " Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" + " float4 posB = gBodies[bIdx].m_pos;\n" + " float4 linVelB = gBodies[bIdx].m_linVel;\n" + " float4 angVelB = gBodies[bIdx].m_angVel;\n" + " float invMassB = gBodies[bIdx].m_invMass;\n" + " Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" + " solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" + " posB, &linVelB, &angVelB, invMassB, invInertiaB );\n" + " if (gBodies[aIdx].m_invMass)\n" + " {\n" + " gBodies[aIdx].m_linVel = linVelA;\n" + " gBodies[aIdx].m_angVel = angVelA;\n" + " } else\n" + " {\n" + " gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);\n" + " gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);\n" + " \n" + " }\n" + " if (gBodies[bIdx].m_invMass)\n" + " {\n" + " gBodies[bIdx].m_linVel = linVelB;\n" + " gBodies[bIdx].m_angVel = angVelB;\n" + " } else\n" + " {\n" + " gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);\n" + " gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);\n" + " \n" + " }\n" + "}\n" + "typedef struct \n" + "{\n" + " int m_valInt0;\n" + " int m_valInt1;\n" + " int m_valInt2;\n" + " int m_valInt3;\n" + " float m_val0;\n" + " float m_val1;\n" + " float m_val2;\n" + " float m_val3;\n" + "} SolverDebugInfo;\n" + "__kernel\n" + "__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void BatchSolveKernelContact(__global Body* gBodies,\n" + " __global Shape* gShapes,\n" + " __global Constraint4* gConstraints,\n" + " __global int* gN,\n" + " __global int* gOffsets,\n" + " __global int* batchSizes,\n" + " int maxBatch1,\n" + " int cellBatch,\n" + " int4 nSplit\n" + " )\n" + "{\n" + " //__local int ldsBatchIdx[WG_SIZE+1];\n" + " __local int ldsCurBatch;\n" + " __local int ldsNextBatch;\n" + " __local int ldsStart;\n" + " int lIdx = GET_LOCAL_IDX;\n" + " int wgIdx = GET_GROUP_IDX;\n" + "// int gIdx = GET_GLOBAL_IDX;\n" + "// debugInfo[gIdx].m_valInt0 = gIdx;\n" + " //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;\n" + " \n" + " \n" + " int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2);\n" + " int remain= (wgIdx%((nSplit.x*nSplit.y)/4));\n" + " int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1);\n" + " int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1);\n" + " int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y);\n" + " //int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1);\n" + " //int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1);\n" + " //int cellIdx = xIdx+yIdx*nSplit;\n" + " \n" + " if( gN[cellIdx] == 0 ) \n" + " return;\n" + " int maxBatch = batchSizes[cellIdx];\n" + " \n" + " \n" + " const int start = gOffsets[cellIdx];\n" + " const int end = start + gN[cellIdx];\n" + " \n" + " \n" + " \n" + " if( lIdx == 0 )\n" + " {\n" + " ldsCurBatch = 0;\n" + " ldsNextBatch = 0;\n" + " ldsStart = start;\n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " int idx=ldsStart+lIdx;\n" + " while (ldsCurBatch < maxBatch)\n" + " {\n" + " for(; idx<end; )\n" + " {\n" + " if (gConstraints[idx].m_batchIdx == ldsCurBatch)\n" + " {\n" + " solveContactConstraint( gBodies, gShapes, &gConstraints[idx] );\n" + " idx+=64;\n" + " } else\n" + " {\n" + " break;\n" + " }\n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " \n" + " if( lIdx == 0 )\n" + " {\n" + " ldsCurBatch++;\n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " }\n" + " \n" + " \n" + "}\n" + "__kernel void solveSingleContactKernel(__global Body* gBodies,\n" + " __global Shape* gShapes,\n" + " __global Constraint4* gConstraints,\n" + " int cellIdx,\n" + " int batchOffset,\n" + " int numConstraintsInBatch\n" + " )\n" + "{\n" + " int index = get_global_id(0);\n" + " if (index < numConstraintsInBatch)\n" + " {\n" + " int idx=batchOffset+index;\n" + " solveContactConstraint( gBodies, gShapes, &gConstraints[idx] );\n" + " } \n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveFriction.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveFriction.h index eb58674f22..9707cdb25d 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveFriction.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveFriction.h @@ -1,421 +1,420 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* solveFrictionCL= \ -"/*\n" -"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Takahiro Harada\n" -"//#pragma OPENCL EXTENSION cl_amd_printf : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" -"#ifdef cl_ext_atomic_counters_32\n" -"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" -"#else\n" -"#define counter32_t volatile global int*\n" -"#endif\n" -"typedef unsigned int u32;\n" -"typedef unsigned short u16;\n" -"typedef unsigned char u8;\n" -"#define GET_GROUP_IDX get_group_id(0)\n" -"#define GET_LOCAL_IDX get_local_id(0)\n" -"#define GET_GLOBAL_IDX get_global_id(0)\n" -"#define GET_GROUP_SIZE get_local_size(0)\n" -"#define GET_NUM_GROUPS get_num_groups(0)\n" -"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" -"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" -"#define AtomInc(x) atom_inc(&(x))\n" -"#define AtomInc1(x, out) out = atom_inc(&(x))\n" -"#define AppendInc(x, out) out = atomic_inc(x)\n" -"#define AtomAdd(x, value) atom_add(&(x), value)\n" -"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" -"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" -"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" -"#define mymake_float4 (float4)\n" -"//#define make_float2 (float2)\n" -"//#define make_uint4 (uint4)\n" -"//#define make_int4 (int4)\n" -"//#define make_uint2 (uint2)\n" -"//#define make_int2 (int2)\n" -"#define max2 max\n" -"#define min2 min\n" -"///////////////////////////////////////\n" -"// Vector\n" -"///////////////////////////////////////\n" -"__inline\n" -"float4 fastNormalize4(float4 v)\n" -"{\n" -" return fast_normalize(v);\n" -"}\n" -"__inline\n" -"float4 cross3(float4 a, float4 b)\n" -"{\n" -" return cross(a,b);\n" -"}\n" -"__inline\n" -"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" -"__inline\n" -"float4 normalize3(const float4 a)\n" -"{\n" -" float4 n = mymake_float4(a.x, a.y, a.z, 0.f);\n" -" return fastNormalize4( n );\n" -"// float length = sqrtf(dot3F4(a, a));\n" -"// return 1.f/length * a;\n" -"}\n" -"///////////////////////////////////////\n" -"// Matrix3x3\n" -"///////////////////////////////////////\n" -"typedef struct\n" -"{\n" -" float4 m_row[3];\n" -"}Matrix3x3;\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b);\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b);\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b)\n" -"{\n" -" float4 ans;\n" -" ans.x = dot3F4( a.m_row[0], b );\n" -" ans.y = dot3F4( a.m_row[1], b );\n" -" ans.z = dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b)\n" -"{\n" -" float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" float4 ans;\n" -" ans.x = dot3F4( a, colx );\n" -" ans.y = dot3F4( a, coly );\n" -" ans.z = dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"///////////////////////////////////////\n" -"// Quaternion\n" -"///////////////////////////////////////\n" -"typedef float4 Quaternion;\n" -"#define WG_SIZE 64\n" -"typedef struct\n" -"{\n" -" float4 m_pos;\n" -" Quaternion m_quat;\n" -" float4 m_linVel;\n" -" float4 m_angVel;\n" -" u32 m_shapeIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"} Body;\n" -"typedef struct\n" -"{\n" -" Matrix3x3 m_invInertia;\n" -" Matrix3x3 m_initInvInertia;\n" -"} Shape;\n" -"typedef struct\n" -"{\n" -" float4 m_linear;\n" -" float4 m_worldPos[4];\n" -" float4 m_center; \n" -" float m_jacCoeffInv[4];\n" -" float m_b[4];\n" -" float m_appliedRambdaDt[4];\n" -" float m_fJacCoeffInv[2]; \n" -" float m_fAppliedRambdaDt[2]; \n" -" u32 m_bodyA;\n" -" u32 m_bodyB;\n" -" int m_batchIdx;\n" -" u32 m_paddings[1];\n" -"} Constraint4;\n" -"typedef struct\n" -"{\n" -" int m_nConstraints;\n" -" int m_start;\n" -" int m_batchIdx;\n" -" int m_nSplit;\n" -"// int m_paddings[1];\n" -"} ConstBuffer;\n" -"typedef struct\n" -"{\n" -" int m_solveFriction;\n" -" int m_maxBatch; // long batch really kills the performance\n" -" int m_batchIdx;\n" -" int m_nSplit;\n" -"// int m_paddings[1];\n" -"} ConstBufferBatchSolve;\n" -"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1);\n" -"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n" -"{\n" -" *linear = mymake_float4(-n.xyz,0.f);\n" -" *angular0 = -cross3(r0, n);\n" -" *angular1 = cross3(r1, n);\n" -"}\n" -"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 );\n" -"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n" -"{\n" -" return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n" -"}\n" -"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" -" float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1);\n" -"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" -" float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)\n" -"{\n" -" // linear0,1 are normlized\n" -" float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n" -" float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" -" float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n" -" float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" -" return -1.f/(jmj0+jmj1+jmj2+jmj3);\n" -"}\n" -"void btPlaneSpace1 (const float4* n, float4* p, float4* q);\n" -" void btPlaneSpace1 (const float4* n, float4* p, float4* q)\n" -"{\n" -" if (fabs(n[0].z) > 0.70710678f) {\n" -" // choose p in y-z plane\n" -" float a = n[0].y*n[0].y + n[0].z*n[0].z;\n" -" float k = 1.f/sqrt(a);\n" -" p[0].x = 0;\n" -" p[0].y = -n[0].z*k;\n" -" p[0].z = n[0].y*k;\n" -" // set q = n x p\n" -" q[0].x = a*k;\n" -" q[0].y = -n[0].x*p[0].z;\n" -" q[0].z = n[0].x*p[0].y;\n" -" }\n" -" else {\n" -" // choose p in x-y plane\n" -" float a = n[0].x*n[0].x + n[0].y*n[0].y;\n" -" float k = 1.f/sqrt(a);\n" -" p[0].x = -n[0].y*k;\n" -" p[0].y = n[0].x*k;\n" -" p[0].z = 0;\n" -" // set q = n x p\n" -" q[0].x = -n[0].z*p[0].y;\n" -" q[0].y = n[0].z*p[0].x;\n" -" q[0].z = a*k;\n" -" }\n" -"}\n" -"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);\n" -"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)\n" -"{\n" -" float frictionCoeff = ldsCs[0].m_linear.w;\n" -" int aIdx = ldsCs[0].m_bodyA;\n" -" int bIdx = ldsCs[0].m_bodyB;\n" -" float4 posA = gBodies[aIdx].m_pos;\n" -" float4 linVelA = gBodies[aIdx].m_linVel;\n" -" float4 angVelA = gBodies[aIdx].m_angVel;\n" -" float invMassA = gBodies[aIdx].m_invMass;\n" -" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" -" float4 posB = gBodies[bIdx].m_pos;\n" -" float4 linVelB = gBodies[bIdx].m_linVel;\n" -" float4 angVelB = gBodies[bIdx].m_angVel;\n" -" float invMassB = gBodies[bIdx].m_invMass;\n" -" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" -" \n" -" {\n" -" float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};\n" -" float minRambdaDt[4] = {0.f,0.f,0.f,0.f};\n" -" float sum = 0;\n" -" for(int j=0; j<4; j++)\n" -" {\n" -" sum +=ldsCs[0].m_appliedRambdaDt[j];\n" -" }\n" -" frictionCoeff = 0.7f;\n" -" for(int j=0; j<4; j++)\n" -" {\n" -" maxRambdaDt[j] = frictionCoeff*sum;\n" -" minRambdaDt[j] = -maxRambdaDt[j];\n" -" }\n" -" \n" -"// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" -"// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );\n" -" \n" -" \n" -" {\n" -" \n" -" __global Constraint4* cs = ldsCs;\n" -" \n" -" if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n" -" const float4 center = cs->m_center;\n" -" \n" -" float4 n = -cs->m_linear;\n" -" \n" -" float4 tangent[2];\n" -" btPlaneSpace1(&n,&tangent[0],&tangent[1]);\n" -" float4 angular0, angular1, linear;\n" -" float4 r0 = center - posA;\n" -" float4 r1 = center - posB;\n" -" for(int i=0; i<2; i++)\n" -" {\n" -" setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n" -" float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n" -" linVelA, angVelA, linVelB, angVelB );\n" -" rambdaDt *= cs->m_fJacCoeffInv[i];\n" -" \n" -" {\n" -" float prevSum = cs->m_fAppliedRambdaDt[i];\n" -" float updated = prevSum;\n" -" updated += rambdaDt;\n" -" updated = max2( updated, minRambdaDt[i] );\n" -" updated = min2( updated, maxRambdaDt[i] );\n" -" rambdaDt = updated - prevSum;\n" -" cs->m_fAppliedRambdaDt[i] = updated;\n" -" }\n" -" \n" -" float4 linImp0 = invMassA*linear*rambdaDt;\n" -" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" -" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" -" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" -" \n" -" linVelA += linImp0;\n" -" angVelA += angImp0;\n" -" linVelB += linImp1;\n" -" angVelB += angImp1;\n" -" }\n" -" { // angular damping for point constraint\n" -" float4 ab = normalize3( posB - posA );\n" -" float4 ac = normalize3( center - posA );\n" -" if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))\n" -" {\n" -" float angNA = dot3F4( n, angVelA );\n" -" float angNB = dot3F4( n, angVelB );\n" -" \n" -" angVelA -= (angNA*0.1f)*n;\n" -" angVelB -= (angNB*0.1f)*n;\n" -" }\n" -" }\n" -" }\n" -" \n" -" \n" -" }\n" -" if (gBodies[aIdx].m_invMass)\n" -" {\n" -" gBodies[aIdx].m_linVel = linVelA;\n" -" gBodies[aIdx].m_angVel = angVelA;\n" -" } else\n" -" {\n" -" gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);\n" -" gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);\n" -" }\n" -" if (gBodies[bIdx].m_invMass)\n" -" {\n" -" gBodies[bIdx].m_linVel = linVelB;\n" -" gBodies[bIdx].m_angVel = angVelB;\n" -" } else\n" -" {\n" -" gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);\n" -" gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);\n" -" }\n" -" \n" -"}\n" -"typedef struct \n" -"{\n" -" int m_valInt0;\n" -" int m_valInt1;\n" -" int m_valInt2;\n" -" int m_valInt3;\n" -" float m_val0;\n" -" float m_val1;\n" -" float m_val2;\n" -" float m_val3;\n" -"} SolverDebugInfo;\n" -"__kernel\n" -"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void BatchSolveKernelFriction(__global Body* gBodies,\n" -" __global Shape* gShapes,\n" -" __global Constraint4* gConstraints,\n" -" __global int* gN,\n" -" __global int* gOffsets,\n" -" __global int* batchSizes,\n" -" int maxBatch1,\n" -" int cellBatch,\n" -" int4 nSplit\n" -" )\n" -"{\n" -" //__local int ldsBatchIdx[WG_SIZE+1];\n" -" __local int ldsCurBatch;\n" -" __local int ldsNextBatch;\n" -" __local int ldsStart;\n" -" int lIdx = GET_LOCAL_IDX;\n" -" int wgIdx = GET_GROUP_IDX;\n" -"// int gIdx = GET_GLOBAL_IDX;\n" -"// debugInfo[gIdx].m_valInt0 = gIdx;\n" -" //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;\n" -" int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2);\n" -" int remain= (wgIdx%((nSplit.x*nSplit.y)/4));\n" -" int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1);\n" -" int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1);\n" -" int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y);\n" -" \n" -" if( gN[cellIdx] == 0 ) \n" -" return;\n" -" int maxBatch = batchSizes[cellIdx];\n" -" const int start = gOffsets[cellIdx];\n" -" const int end = start + gN[cellIdx];\n" -" \n" -" if( lIdx == 0 )\n" -" {\n" -" ldsCurBatch = 0;\n" -" ldsNextBatch = 0;\n" -" ldsStart = start;\n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" int idx=ldsStart+lIdx;\n" -" while (ldsCurBatch < maxBatch)\n" -" {\n" -" for(; idx<end; )\n" -" {\n" -" if (gConstraints[idx].m_batchIdx == ldsCurBatch)\n" -" {\n" -" solveFrictionConstraint( gBodies, gShapes, &gConstraints[idx] );\n" -" idx+=64;\n" -" } else\n" -" {\n" -" break;\n" -" }\n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" if( lIdx == 0 )\n" -" {\n" -" ldsCurBatch++;\n" -" }\n" -" GROUP_LDS_BARRIER;\n" -" }\n" -" \n" -" \n" -"}\n" -"__kernel void solveSingleFrictionKernel(__global Body* gBodies,\n" -" __global Shape* gShapes,\n" -" __global Constraint4* gConstraints,\n" -" int cellIdx,\n" -" int batchOffset,\n" -" int numConstraintsInBatch\n" -" )\n" -"{\n" -" int index = get_global_id(0);\n" -" if (index < numConstraintsInBatch)\n" -" {\n" -" \n" -" int idx=batchOffset+index;\n" -" \n" -" solveFrictionConstraint( gBodies, gShapes, &gConstraints[idx] );\n" -" } \n" -"}\n" -; +static const char* solveFrictionCL = + "/*\n" + "Copyright (c) 2012 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Takahiro Harada\n" + "//#pragma OPENCL EXTENSION cl_amd_printf : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" + "#ifdef cl_ext_atomic_counters_32\n" + "#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" + "#else\n" + "#define counter32_t volatile global int*\n" + "#endif\n" + "typedef unsigned int u32;\n" + "typedef unsigned short u16;\n" + "typedef unsigned char u8;\n" + "#define GET_GROUP_IDX get_group_id(0)\n" + "#define GET_LOCAL_IDX get_local_id(0)\n" + "#define GET_GLOBAL_IDX get_global_id(0)\n" + "#define GET_GROUP_SIZE get_local_size(0)\n" + "#define GET_NUM_GROUPS get_num_groups(0)\n" + "#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" + "#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" + "#define AtomInc(x) atom_inc(&(x))\n" + "#define AtomInc1(x, out) out = atom_inc(&(x))\n" + "#define AppendInc(x, out) out = atomic_inc(x)\n" + "#define AtomAdd(x, value) atom_add(&(x), value)\n" + "#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" + "#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" + "#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" + "#define mymake_float4 (float4)\n" + "//#define make_float2 (float2)\n" + "//#define make_uint4 (uint4)\n" + "//#define make_int4 (int4)\n" + "//#define make_uint2 (uint2)\n" + "//#define make_int2 (int2)\n" + "#define max2 max\n" + "#define min2 min\n" + "///////////////////////////////////////\n" + "// Vector\n" + "///////////////////////////////////////\n" + "__inline\n" + "float4 fastNormalize4(float4 v)\n" + "{\n" + " return fast_normalize(v);\n" + "}\n" + "__inline\n" + "float4 cross3(float4 a, float4 b)\n" + "{\n" + " return cross(a,b);\n" + "}\n" + "__inline\n" + "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" + "__inline\n" + "float4 normalize3(const float4 a)\n" + "{\n" + " float4 n = mymake_float4(a.x, a.y, a.z, 0.f);\n" + " return fastNormalize4( n );\n" + "// float length = sqrtf(dot3F4(a, a));\n" + "// return 1.f/length * a;\n" + "}\n" + "///////////////////////////////////////\n" + "// Matrix3x3\n" + "///////////////////////////////////////\n" + "typedef struct\n" + "{\n" + " float4 m_row[3];\n" + "}Matrix3x3;\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b);\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b);\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b)\n" + "{\n" + " float4 ans;\n" + " ans.x = dot3F4( a.m_row[0], b );\n" + " ans.y = dot3F4( a.m_row[1], b );\n" + " ans.z = dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b)\n" + "{\n" + " float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " float4 ans;\n" + " ans.x = dot3F4( a, colx );\n" + " ans.y = dot3F4( a, coly );\n" + " ans.z = dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "///////////////////////////////////////\n" + "// Quaternion\n" + "///////////////////////////////////////\n" + "typedef float4 Quaternion;\n" + "#define WG_SIZE 64\n" + "typedef struct\n" + "{\n" + " float4 m_pos;\n" + " Quaternion m_quat;\n" + " float4 m_linVel;\n" + " float4 m_angVel;\n" + " u32 m_shapeIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "} Body;\n" + "typedef struct\n" + "{\n" + " Matrix3x3 m_invInertia;\n" + " Matrix3x3 m_initInvInertia;\n" + "} Shape;\n" + "typedef struct\n" + "{\n" + " float4 m_linear;\n" + " float4 m_worldPos[4];\n" + " float4 m_center; \n" + " float m_jacCoeffInv[4];\n" + " float m_b[4];\n" + " float m_appliedRambdaDt[4];\n" + " float m_fJacCoeffInv[2]; \n" + " float m_fAppliedRambdaDt[2]; \n" + " u32 m_bodyA;\n" + " u32 m_bodyB;\n" + " int m_batchIdx;\n" + " u32 m_paddings[1];\n" + "} Constraint4;\n" + "typedef struct\n" + "{\n" + " int m_nConstraints;\n" + " int m_start;\n" + " int m_batchIdx;\n" + " int m_nSplit;\n" + "// int m_paddings[1];\n" + "} ConstBuffer;\n" + "typedef struct\n" + "{\n" + " int m_solveFriction;\n" + " int m_maxBatch; // long batch really kills the performance\n" + " int m_batchIdx;\n" + " int m_nSplit;\n" + "// int m_paddings[1];\n" + "} ConstBufferBatchSolve;\n" + "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1);\n" + "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n" + "{\n" + " *linear = mymake_float4(-n.xyz,0.f);\n" + " *angular0 = -cross3(r0, n);\n" + " *angular1 = cross3(r1, n);\n" + "}\n" + "float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 );\n" + "float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n" + "{\n" + " return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n" + "}\n" + "float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" + " float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1);\n" + "float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" + " float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)\n" + "{\n" + " // linear0,1 are normlized\n" + " float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n" + " float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" + " float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n" + " float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" + " return -1.f/(jmj0+jmj1+jmj2+jmj3);\n" + "}\n" + "void btPlaneSpace1 (const float4* n, float4* p, float4* q);\n" + " void btPlaneSpace1 (const float4* n, float4* p, float4* q)\n" + "{\n" + " if (fabs(n[0].z) > 0.70710678f) {\n" + " // choose p in y-z plane\n" + " float a = n[0].y*n[0].y + n[0].z*n[0].z;\n" + " float k = 1.f/sqrt(a);\n" + " p[0].x = 0;\n" + " p[0].y = -n[0].z*k;\n" + " p[0].z = n[0].y*k;\n" + " // set q = n x p\n" + " q[0].x = a*k;\n" + " q[0].y = -n[0].x*p[0].z;\n" + " q[0].z = n[0].x*p[0].y;\n" + " }\n" + " else {\n" + " // choose p in x-y plane\n" + " float a = n[0].x*n[0].x + n[0].y*n[0].y;\n" + " float k = 1.f/sqrt(a);\n" + " p[0].x = -n[0].y*k;\n" + " p[0].y = n[0].x*k;\n" + " p[0].z = 0;\n" + " // set q = n x p\n" + " q[0].x = -n[0].z*p[0].y;\n" + " q[0].y = n[0].z*p[0].x;\n" + " q[0].z = a*k;\n" + " }\n" + "}\n" + "void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);\n" + "void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)\n" + "{\n" + " float frictionCoeff = ldsCs[0].m_linear.w;\n" + " int aIdx = ldsCs[0].m_bodyA;\n" + " int bIdx = ldsCs[0].m_bodyB;\n" + " float4 posA = gBodies[aIdx].m_pos;\n" + " float4 linVelA = gBodies[aIdx].m_linVel;\n" + " float4 angVelA = gBodies[aIdx].m_angVel;\n" + " float invMassA = gBodies[aIdx].m_invMass;\n" + " Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" + " float4 posB = gBodies[bIdx].m_pos;\n" + " float4 linVelB = gBodies[bIdx].m_linVel;\n" + " float4 angVelB = gBodies[bIdx].m_angVel;\n" + " float invMassB = gBodies[bIdx].m_invMass;\n" + " Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" + " \n" + " {\n" + " float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};\n" + " float minRambdaDt[4] = {0.f,0.f,0.f,0.f};\n" + " float sum = 0;\n" + " for(int j=0; j<4; j++)\n" + " {\n" + " sum +=ldsCs[0].m_appliedRambdaDt[j];\n" + " }\n" + " frictionCoeff = 0.7f;\n" + " for(int j=0; j<4; j++)\n" + " {\n" + " maxRambdaDt[j] = frictionCoeff*sum;\n" + " minRambdaDt[j] = -maxRambdaDt[j];\n" + " }\n" + " \n" + "// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" + "// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );\n" + " \n" + " \n" + " {\n" + " \n" + " __global Constraint4* cs = ldsCs;\n" + " \n" + " if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n" + " const float4 center = cs->m_center;\n" + " \n" + " float4 n = -cs->m_linear;\n" + " \n" + " float4 tangent[2];\n" + " btPlaneSpace1(&n,&tangent[0],&tangent[1]);\n" + " float4 angular0, angular1, linear;\n" + " float4 r0 = center - posA;\n" + " float4 r1 = center - posB;\n" + " for(int i=0; i<2; i++)\n" + " {\n" + " setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n" + " float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n" + " linVelA, angVelA, linVelB, angVelB );\n" + " rambdaDt *= cs->m_fJacCoeffInv[i];\n" + " \n" + " {\n" + " float prevSum = cs->m_fAppliedRambdaDt[i];\n" + " float updated = prevSum;\n" + " updated += rambdaDt;\n" + " updated = max2( updated, minRambdaDt[i] );\n" + " updated = min2( updated, maxRambdaDt[i] );\n" + " rambdaDt = updated - prevSum;\n" + " cs->m_fAppliedRambdaDt[i] = updated;\n" + " }\n" + " \n" + " float4 linImp0 = invMassA*linear*rambdaDt;\n" + " float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" + " float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" + " float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" + " \n" + " linVelA += linImp0;\n" + " angVelA += angImp0;\n" + " linVelB += linImp1;\n" + " angVelB += angImp1;\n" + " }\n" + " { // angular damping for point constraint\n" + " float4 ab = normalize3( posB - posA );\n" + " float4 ac = normalize3( center - posA );\n" + " if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))\n" + " {\n" + " float angNA = dot3F4( n, angVelA );\n" + " float angNB = dot3F4( n, angVelB );\n" + " \n" + " angVelA -= (angNA*0.1f)*n;\n" + " angVelB -= (angNB*0.1f)*n;\n" + " }\n" + " }\n" + " }\n" + " \n" + " \n" + " }\n" + " if (gBodies[aIdx].m_invMass)\n" + " {\n" + " gBodies[aIdx].m_linVel = linVelA;\n" + " gBodies[aIdx].m_angVel = angVelA;\n" + " } else\n" + " {\n" + " gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);\n" + " gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);\n" + " }\n" + " if (gBodies[bIdx].m_invMass)\n" + " {\n" + " gBodies[bIdx].m_linVel = linVelB;\n" + " gBodies[bIdx].m_angVel = angVelB;\n" + " } else\n" + " {\n" + " gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);\n" + " gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);\n" + " }\n" + " \n" + "}\n" + "typedef struct \n" + "{\n" + " int m_valInt0;\n" + " int m_valInt1;\n" + " int m_valInt2;\n" + " int m_valInt3;\n" + " float m_val0;\n" + " float m_val1;\n" + " float m_val2;\n" + " float m_val3;\n" + "} SolverDebugInfo;\n" + "__kernel\n" + "__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void BatchSolveKernelFriction(__global Body* gBodies,\n" + " __global Shape* gShapes,\n" + " __global Constraint4* gConstraints,\n" + " __global int* gN,\n" + " __global int* gOffsets,\n" + " __global int* batchSizes,\n" + " int maxBatch1,\n" + " int cellBatch,\n" + " int4 nSplit\n" + " )\n" + "{\n" + " //__local int ldsBatchIdx[WG_SIZE+1];\n" + " __local int ldsCurBatch;\n" + " __local int ldsNextBatch;\n" + " __local int ldsStart;\n" + " int lIdx = GET_LOCAL_IDX;\n" + " int wgIdx = GET_GROUP_IDX;\n" + "// int gIdx = GET_GLOBAL_IDX;\n" + "// debugInfo[gIdx].m_valInt0 = gIdx;\n" + " //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;\n" + " int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2);\n" + " int remain= (wgIdx%((nSplit.x*nSplit.y)/4));\n" + " int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1);\n" + " int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1);\n" + " int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y);\n" + " \n" + " if( gN[cellIdx] == 0 ) \n" + " return;\n" + " int maxBatch = batchSizes[cellIdx];\n" + " const int start = gOffsets[cellIdx];\n" + " const int end = start + gN[cellIdx];\n" + " \n" + " if( lIdx == 0 )\n" + " {\n" + " ldsCurBatch = 0;\n" + " ldsNextBatch = 0;\n" + " ldsStart = start;\n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " int idx=ldsStart+lIdx;\n" + " while (ldsCurBatch < maxBatch)\n" + " {\n" + " for(; idx<end; )\n" + " {\n" + " if (gConstraints[idx].m_batchIdx == ldsCurBatch)\n" + " {\n" + " solveFrictionConstraint( gBodies, gShapes, &gConstraints[idx] );\n" + " idx+=64;\n" + " } else\n" + " {\n" + " break;\n" + " }\n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " if( lIdx == 0 )\n" + " {\n" + " ldsCurBatch++;\n" + " }\n" + " GROUP_LDS_BARRIER;\n" + " }\n" + " \n" + " \n" + "}\n" + "__kernel void solveSingleFrictionKernel(__global Body* gBodies,\n" + " __global Shape* gShapes,\n" + " __global Constraint4* gConstraints,\n" + " int cellIdx,\n" + " int batchOffset,\n" + " int numConstraintsInBatch\n" + " )\n" + "{\n" + " int index = get_global_id(0);\n" + " if (index < numConstraintsInBatch)\n" + " {\n" + " \n" + " int idx=batchOffset+index;\n" + " \n" + " solveFrictionConstraint( gBodies, gShapes, &gConstraints[idx] );\n" + " } \n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup.h index eb1834ee00..d53db03181 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup.h @@ -1,703 +1,702 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* solverSetupCL= \ -"/*\n" -"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Takahiro Harada\n" -"#ifndef B3_CONTACT4DATA_H\n" -"#define B3_CONTACT4DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#define B3_FLOAT4_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#define B3_PLATFORM_DEFINITIONS_H\n" -"struct MyTest\n" -"{\n" -" int bla;\n" -"};\n" -"#ifdef __cplusplus\n" -"#else\n" -"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" -"#define B3_LARGE_FLOAT 1e18f\n" -"#define B3_INFINITY 1e18f\n" -"#define b3Assert(a)\n" -"#define b3ConstArray(a) __global const a*\n" -"#define b3AtomicInc atomic_inc\n" -"#define b3AtomicAdd atomic_add\n" -"#define b3Fabs fabs\n" -"#define b3Sqrt native_sqrt\n" -"#define b3Sin native_sin\n" -"#define b3Cos native_cos\n" -"#define B3_STATIC\n" -"#endif\n" -"#endif\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Float4;\n" -" #define b3Float4ConstArg const b3Float4\n" -" #define b3MakeFloat4 (float4)\n" -" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return dot(a1, b1);\n" -" }\n" -" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return cross(a1, b1);\n" -" }\n" -" #define b3MinFloat4 min\n" -" #define b3MaxFloat4 max\n" -" #define b3Normalized(a) normalize(a)\n" -"#endif \n" -" \n" -"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" -"{\n" -" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" -" return false;\n" -" return true;\n" -"}\n" -"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" -"{\n" -" float maxDot = -B3_INFINITY;\n" -" int i = 0;\n" -" int ptIndex = -1;\n" -" for( i = 0; i < vecLen; i++ )\n" -" {\n" -" float dot = b3Dot3F4(vecArray[i],vec);\n" -" \n" -" if( dot > maxDot )\n" -" {\n" -" maxDot = dot;\n" -" ptIndex = i;\n" -" }\n" -" }\n" -" b3Assert(ptIndex>=0);\n" -" if (ptIndex<0)\n" -" {\n" -" ptIndex = 0;\n" -" }\n" -" *dotOut = maxDot;\n" -" return ptIndex;\n" -"}\n" -"#endif //B3_FLOAT4_H\n" -"typedef struct b3Contact4Data b3Contact4Data_t;\n" -"struct b3Contact4Data\n" -"{\n" -" b3Float4 m_worldPosB[4];\n" -"// b3Float4 m_localPosA[4];\n" -"// b3Float4 m_localPosB[4];\n" -" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" -" unsigned short m_restituitionCoeffCmp;\n" -" unsigned short m_frictionCoeffCmp;\n" -" int m_batchIdx;\n" -" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" -" int m_bodyBPtrAndSignBit;\n" -" int m_childIndexA;\n" -" int m_childIndexB;\n" -" int m_unused1;\n" -" int m_unused2;\n" -"};\n" -"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" -"{\n" -" return (int)contact->m_worldNormalOnB.w;\n" -"};\n" -"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" -"{\n" -" contact->m_worldNormalOnB.w = (float)numPoints;\n" -"};\n" -"#endif //B3_CONTACT4DATA_H\n" -"#ifndef B3_CONTACT_CONSTRAINT5_H\n" -"#define B3_CONTACT_CONSTRAINT5_H\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"typedef struct b3ContactConstraint4 b3ContactConstraint4_t;\n" -"struct b3ContactConstraint4\n" -"{\n" -" b3Float4 m_linear;//normal?\n" -" b3Float4 m_worldPos[4];\n" -" b3Float4 m_center; // friction\n" -" float m_jacCoeffInv[4];\n" -" float m_b[4];\n" -" float m_appliedRambdaDt[4];\n" -" float m_fJacCoeffInv[2]; // friction\n" -" float m_fAppliedRambdaDt[2]; // friction\n" -" unsigned int m_bodyA;\n" -" unsigned int m_bodyB;\n" -" int m_batchIdx;\n" -" unsigned int m_paddings;\n" -"};\n" -"//inline void setFrictionCoeff(float value) { m_linear[3] = value; }\n" -"inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) \n" -"{\n" -" return constraint->m_linear.w; \n" -"}\n" -"#endif //B3_CONTACT_CONSTRAINT5_H\n" -"#ifndef B3_RIGIDBODY_DATA_H\n" -"#define B3_RIGIDBODY_DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_QUAT_H\n" -"#define B3_QUAT_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif\n" -"#endif\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Quat;\n" -" #define b3QuatConstArg const b3Quat\n" -" \n" -" \n" -"inline float4 b3FastNormalize4(float4 v)\n" -"{\n" -" v = (float4)(v.xyz,0.f);\n" -" return fast_normalize(v);\n" -"}\n" -" \n" -"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" -"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" -"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" -"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" -"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" -"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" -"{\n" -" b3Quat ans;\n" -" ans = b3Cross3( a, b );\n" -" ans += a.w*b+b.w*a;\n" -"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" -" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" -" return ans;\n" -"}\n" -"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" -"{\n" -" b3Quat q;\n" -" q=in;\n" -" //return b3FastNormalize4(in);\n" -" float len = native_sqrt(dot(q, q));\n" -" if(len > 0.f)\n" -" {\n" -" q *= 1.f / len;\n" -" }\n" -" else\n" -" {\n" -" q.x = q.y = q.z = 0.f;\n" -" q.w = 1.f;\n" -" }\n" -" return q;\n" -"}\n" -"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" -"{\n" -" b3Quat qInv = b3QuatInvert( q );\n" -" float4 vcpy = vec;\n" -" vcpy.w = 0.f;\n" -" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" -" return out;\n" -"}\n" -"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" -"{\n" -" return (b3Quat)(-q.xyz, q.w);\n" -"}\n" -"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" -"{\n" -" return (b3Quat)(-q.xyz, q.w);\n" -"}\n" -"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" -"{\n" -" return b3QuatRotate( b3QuatInvert( q ), vec );\n" -"}\n" -"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" -"{\n" -" return b3QuatRotate( orientation, point ) + (translation);\n" -"}\n" -" \n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"#ifndef B3_MAT3x3_H\n" -"#define B3_MAT3x3_H\n" -"#ifndef B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"typedef struct\n" -"{\n" -" b3Float4 m_row[3];\n" -"}b3Mat3x3;\n" -"#define b3Mat3x3ConstArg const b3Mat3x3\n" -"#define b3GetRow(m,row) (m.m_row[row])\n" -"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" -"{\n" -" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" -" b3Mat3x3 out;\n" -" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" -" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" -" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" -" out.m_row[0].w = 0.f;\n" -" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" -" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" -" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" -" out.m_row[1].w = 0.f;\n" -" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" -" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" -" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" -" out.m_row[2].w = 0.f;\n" -" return out;\n" -"}\n" -"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" -"{\n" -" b3Mat3x3 out;\n" -" out.m_row[0] = fabs(matIn.m_row[0]);\n" -" out.m_row[1] = fabs(matIn.m_row[1]);\n" -" out.m_row[2] = fabs(matIn.m_row[2]);\n" -" return out;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtZero();\n" -"__inline\n" -"b3Mat3x3 mtIdentity();\n" -"__inline\n" -"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" -"__inline\n" -"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" -"__inline\n" -"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" -"__inline\n" -"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" -"__inline\n" -"b3Mat3x3 mtZero()\n" -"{\n" -" b3Mat3x3 m;\n" -" m.m_row[0] = (b3Float4)(0.f);\n" -" m.m_row[1] = (b3Float4)(0.f);\n" -" m.m_row[2] = (b3Float4)(0.f);\n" -" return m;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtIdentity()\n" -"{\n" -" b3Mat3x3 m;\n" -" m.m_row[0] = (b3Float4)(1,0,0,0);\n" -" m.m_row[1] = (b3Float4)(0,1,0,0);\n" -" m.m_row[2] = (b3Float4)(0,0,1,0);\n" -" return m;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" -"{\n" -" b3Mat3x3 out;\n" -" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" -" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" -" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" -" return out;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" -"{\n" -" b3Mat3x3 transB;\n" -" transB = mtTranspose( b );\n" -" b3Mat3x3 ans;\n" -" // why this doesn't run when 0ing in the for{}\n" -" a.m_row[0].w = 0.f;\n" -" a.m_row[1].w = 0.f;\n" -" a.m_row[2].w = 0.f;\n" -" for(int i=0; i<3; i++)\n" -" {\n" -"// a.m_row[i].w = 0.f;\n" -" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" -" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" -" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" -" ans.m_row[i].w = 0.f;\n" -" }\n" -" return ans;\n" -"}\n" -"__inline\n" -"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" -"{\n" -" b3Float4 ans;\n" -" ans.x = b3Dot3F4( a.m_row[0], b );\n" -" ans.y = b3Dot3F4( a.m_row[1], b );\n" -" ans.z = b3Dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" -"{\n" -" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" b3Float4 ans;\n" -" ans.x = b3Dot3F4( a, colx );\n" -" ans.y = b3Dot3F4( a, coly );\n" -" ans.z = b3Dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"#endif\n" -"#endif //B3_MAT3x3_H\n" -"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" -"struct b3RigidBodyData\n" -"{\n" -" b3Float4 m_pos;\n" -" b3Quat m_quat;\n" -" b3Float4 m_linVel;\n" -" b3Float4 m_angVel;\n" -" int m_collidableIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"};\n" -"typedef struct b3InertiaData b3InertiaData_t;\n" -"struct b3InertiaData\n" -"{\n" -" b3Mat3x3 m_invInertiaWorld;\n" -" b3Mat3x3 m_initInvInertia;\n" -"};\n" -"#endif //B3_RIGIDBODY_DATA_H\n" -" \n" -"void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);\n" -" void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q)\n" -"{\n" -" if (b3Fabs(n.z) > 0.70710678f) {\n" -" // choose p in y-z plane\n" -" float a = n.y*n.y + n.z*n.z;\n" -" float k = 1.f/sqrt(a);\n" -" p[0].x = 0;\n" -" p[0].y = -n.z*k;\n" -" p[0].z = n.y*k;\n" -" // set q = n x p\n" -" q[0].x = a*k;\n" -" q[0].y = -n.x*p[0].z;\n" -" q[0].z = n.x*p[0].y;\n" -" }\n" -" else {\n" -" // choose p in x-y plane\n" -" float a = n.x*n.x + n.y*n.y;\n" -" float k = 1.f/sqrt(a);\n" -" p[0].x = -n.y*k;\n" -" p[0].y = n.x*k;\n" -" p[0].z = 0;\n" -" // set q = n x p\n" -" q[0].x = -n.z*p[0].y;\n" -" q[0].y = n.z*p[0].x;\n" -" q[0].z = a*k;\n" -" }\n" -"}\n" -" \n" -"void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)\n" -"{\n" -" *linear = b3MakeFloat4(n.x,n.y,n.z,0.f);\n" -" *angular0 = b3Cross3(r0, n);\n" -" *angular1 = -b3Cross3(r1, n);\n" -"}\n" -"float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,\n" -" b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 )\n" -"{\n" -" return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);\n" -"}\n" -"float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,\n" -" float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)\n" -"{\n" -" // linear0,1 are normlized\n" -" float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0;\n" -" float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" -" float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1;\n" -" float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" -" return -1.f/(jmj0+jmj1+jmj2+jmj3);\n" -"}\n" -"void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,\n" -" b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, \n" -" __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,\n" -" b3ContactConstraint4_t* dstC )\n" -"{\n" -" dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);\n" -" dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n" -" float dtInv = 1.f/dt;\n" -" for(int ic=0; ic<4; ic++)\n" -" {\n" -" dstC->m_appliedRambdaDt[ic] = 0.f;\n" -" }\n" -" dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n" -" dstC->m_linear = src->m_worldNormalOnB;\n" -" dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n" -" for(int ic=0; ic<4; ic++)\n" -" {\n" -" b3Float4 r0 = src->m_worldPosB[ic] - posA;\n" -" b3Float4 r1 = src->m_worldPosB[ic] - posB;\n" -" if( ic >= src->m_worldNormalOnB.w )//npoints\n" -" {\n" -" dstC->m_jacCoeffInv[ic] = 0.f;\n" -" continue;\n" -" }\n" -" float relVelN;\n" -" {\n" -" b3Float4 linear, angular0, angular1;\n" -" setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);\n" -" dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n" -" invMassA, &invInertiaA, invMassB, &invInertiaB );\n" -" relVelN = calcRelVel(linear, -linear, angular0, angular1,\n" -" linVelA, angVelA, linVelB, angVelB);\n" -" float e = 0.f;//src->getRestituitionCoeff();\n" -" if( relVelN*relVelN < 0.004f ) e = 0.f;\n" -" dstC->m_b[ic] = e*relVelN;\n" -" //float penetration = src->m_worldPosB[ic].w;\n" -" dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n" -" dstC->m_appliedRambdaDt[ic] = 0.f;\n" -" }\n" -" }\n" -" if( src->m_worldNormalOnB.w > 0 )//npoints\n" -" { // prepare friction\n" -" b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f);\n" -" for(int i=0; i<src->m_worldNormalOnB.w; i++) \n" -" center += src->m_worldPosB[i];\n" -" center /= (float)src->m_worldNormalOnB.w;\n" -" b3Float4 tangent[2];\n" -" b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);\n" -" \n" -" b3Float4 r[2];\n" -" r[0] = center - posA;\n" -" r[1] = center - posB;\n" -" for(int i=0; i<2; i++)\n" -" {\n" -" b3Float4 linear, angular0, angular1;\n" -" setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n" -" dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n" -" invMassA, &invInertiaA, invMassB, &invInertiaB );\n" -" dstC->m_fAppliedRambdaDt[i] = 0.f;\n" -" }\n" -" dstC->m_center = center;\n" -" }\n" -" for(int i=0; i<4; i++)\n" -" {\n" -" if( i<src->m_worldNormalOnB.w )\n" -" {\n" -" dstC->m_worldPos[i] = src->m_worldPosB[i];\n" -" }\n" -" else\n" -" {\n" -" dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f);\n" -" }\n" -" }\n" -"}\n" -"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" -"#ifdef cl_ext_atomic_counters_32\n" -"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" -"#else\n" -"#define counter32_t volatile global int*\n" -"#endif\n" -"typedef unsigned int u32;\n" -"typedef unsigned short u16;\n" -"typedef unsigned char u8;\n" -"#define GET_GROUP_IDX get_group_id(0)\n" -"#define GET_LOCAL_IDX get_local_id(0)\n" -"#define GET_GLOBAL_IDX get_global_id(0)\n" -"#define GET_GROUP_SIZE get_local_size(0)\n" -"#define GET_NUM_GROUPS get_num_groups(0)\n" -"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" -"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" -"#define AtomInc(x) atom_inc(&(x))\n" -"#define AtomInc1(x, out) out = atom_inc(&(x))\n" -"#define AppendInc(x, out) out = atomic_inc(x)\n" -"#define AtomAdd(x, value) atom_add(&(x), value)\n" -"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" -"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" -"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" -"#define make_float4 (float4)\n" -"#define make_float2 (float2)\n" -"#define make_uint4 (uint4)\n" -"#define make_int4 (int4)\n" -"#define make_uint2 (uint2)\n" -"#define make_int2 (int2)\n" -"#define max2 max\n" -"#define min2 min\n" -"///////////////////////////////////////\n" -"// Vector\n" -"///////////////////////////////////////\n" -"__inline\n" -"float fastDiv(float numerator, float denominator)\n" -"{\n" -" return native_divide(numerator, denominator); \n" -"// return numerator/denominator; \n" -"}\n" -"__inline\n" -"float4 fastDiv4(float4 numerator, float4 denominator)\n" -"{\n" -" return native_divide(numerator, denominator); \n" -"}\n" -"__inline\n" -"float fastSqrtf(float f2)\n" -"{\n" -" return native_sqrt(f2);\n" -"// return sqrt(f2);\n" -"}\n" -"__inline\n" -"float fastRSqrt(float f2)\n" -"{\n" -" return native_rsqrt(f2);\n" -"}\n" -"__inline\n" -"float fastLength4(float4 v)\n" -"{\n" -" return fast_length(v);\n" -"}\n" -"__inline\n" -"float4 fastNormalize4(float4 v)\n" -"{\n" -" return fast_normalize(v);\n" -"}\n" -"__inline\n" -"float sqrtf(float a)\n" -"{\n" -"// return sqrt(a);\n" -" return native_sqrt(a);\n" -"}\n" -"__inline\n" -"float4 cross3(float4 a, float4 b)\n" -"{\n" -" return cross(a,b);\n" -"}\n" -"__inline\n" -"float dot3F4(float4 a, float4 b)\n" -"{\n" -" float4 a1 = make_float4(a.xyz,0.f);\n" -" float4 b1 = make_float4(b.xyz,0.f);\n" -" return dot(a1, b1);\n" -"}\n" -"__inline\n" -"float length3(const float4 a)\n" -"{\n" -" return sqrtf(dot3F4(a,a));\n" -"}\n" -"__inline\n" -"float dot4(const float4 a, const float4 b)\n" -"{\n" -" return dot( a, b );\n" -"}\n" -"// for height\n" -"__inline\n" -"float dot3w1(const float4 point, const float4 eqn)\n" -"{\n" -" return dot3F4(point,eqn) + eqn.w;\n" -"}\n" -"__inline\n" -"float4 normalize3(const float4 a)\n" -"{\n" -" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" -" return fastNormalize4( n );\n" -"// float length = sqrtf(dot3F4(a, a));\n" -"// return 1.f/length * a;\n" -"}\n" -"__inline\n" -"float4 normalize4(const float4 a)\n" -"{\n" -" float length = sqrtf(dot4(a, a));\n" -" return 1.f/length * a;\n" -"}\n" -"__inline\n" -"float4 createEquation(const float4 a, const float4 b, const float4 c)\n" -"{\n" -" float4 eqn;\n" -" float4 ab = b-a;\n" -" float4 ac = c-a;\n" -" eqn = normalize3( cross3(ab, ac) );\n" -" eqn.w = -dot3F4(eqn,a);\n" -" return eqn;\n" -"}\n" -"#define WG_SIZE 64\n" -"typedef struct\n" -"{\n" -" int m_nConstraints;\n" -" int m_start;\n" -" int m_batchIdx;\n" -" int m_nSplit;\n" -"// int m_paddings[1];\n" -"} ConstBuffer;\n" -"typedef struct\n" -"{\n" -" int m_solveFriction;\n" -" int m_maxBatch; // long batch really kills the performance\n" -" int m_batchIdx;\n" -" int m_nSplit;\n" -"// int m_paddings[1];\n" -"} ConstBufferBatchSolve;\n" -" \n" -"typedef struct \n" -"{\n" -" int m_valInt0;\n" -" int m_valInt1;\n" -" int m_valInt2;\n" -" int m_valInt3;\n" -" float m_val0;\n" -" float m_val1;\n" -" float m_val2;\n" -" float m_val3;\n" -"} SolverDebugInfo;\n" -"typedef struct\n" -"{\n" -" int m_nContacts;\n" -" float m_dt;\n" -" float m_positionDrift;\n" -" float m_positionConstraintCoeff;\n" -"} ConstBufferCTC;\n" -"__kernel\n" -"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void ContactToConstraintKernel(__global struct b3Contact4Data* gContact, __global b3RigidBodyData_t* gBodies, __global b3InertiaData_t* gShapes, __global b3ContactConstraint4_t* gConstraintOut, \n" -"int nContacts,\n" -"float dt,\n" -"float positionDrift,\n" -"float positionConstraintCoeff\n" -")\n" -"{\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" \n" -" if( gIdx < nContacts )\n" -" {\n" -" int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n" -" int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n" -" float4 posA = gBodies[aIdx].m_pos;\n" -" float4 linVelA = gBodies[aIdx].m_linVel;\n" -" float4 angVelA = gBodies[aIdx].m_angVel;\n" -" float invMassA = gBodies[aIdx].m_invMass;\n" -" b3Mat3x3 invInertiaA = gShapes[aIdx].m_initInvInertia;\n" -" float4 posB = gBodies[bIdx].m_pos;\n" -" float4 linVelB = gBodies[bIdx].m_linVel;\n" -" float4 angVelB = gBodies[bIdx].m_angVel;\n" -" float invMassB = gBodies[bIdx].m_invMass;\n" -" b3Mat3x3 invInertiaB = gShapes[bIdx].m_initInvInertia;\n" -" b3ContactConstraint4_t cs;\n" -" setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n" -" &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,\n" -" &cs );\n" -" \n" -" cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n" -" gConstraintOut[gIdx] = cs;\n" -" }\n" -"}\n" -; +static const char* solverSetupCL = + "/*\n" + "Copyright (c) 2012 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Takahiro Harada\n" + "#ifndef B3_CONTACT4DATA_H\n" + "#define B3_CONTACT4DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#define B3_FLOAT4_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#define B3_PLATFORM_DEFINITIONS_H\n" + "struct MyTest\n" + "{\n" + " int bla;\n" + "};\n" + "#ifdef __cplusplus\n" + "#else\n" + "//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" + "#define B3_LARGE_FLOAT 1e18f\n" + "#define B3_INFINITY 1e18f\n" + "#define b3Assert(a)\n" + "#define b3ConstArray(a) __global const a*\n" + "#define b3AtomicInc atomic_inc\n" + "#define b3AtomicAdd atomic_add\n" + "#define b3Fabs fabs\n" + "#define b3Sqrt native_sqrt\n" + "#define b3Sin native_sin\n" + "#define b3Cos native_cos\n" + "#define B3_STATIC\n" + "#endif\n" + "#endif\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Float4;\n" + " #define b3Float4ConstArg const b3Float4\n" + " #define b3MakeFloat4 (float4)\n" + " float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return dot(a1, b1);\n" + " }\n" + " b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return cross(a1, b1);\n" + " }\n" + " #define b3MinFloat4 min\n" + " #define b3MaxFloat4 max\n" + " #define b3Normalized(a) normalize(a)\n" + "#endif \n" + " \n" + "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" + "{\n" + " if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" + " return false;\n" + " return true;\n" + "}\n" + "inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" + "{\n" + " float maxDot = -B3_INFINITY;\n" + " int i = 0;\n" + " int ptIndex = -1;\n" + " for( i = 0; i < vecLen; i++ )\n" + " {\n" + " float dot = b3Dot3F4(vecArray[i],vec);\n" + " \n" + " if( dot > maxDot )\n" + " {\n" + " maxDot = dot;\n" + " ptIndex = i;\n" + " }\n" + " }\n" + " b3Assert(ptIndex>=0);\n" + " if (ptIndex<0)\n" + " {\n" + " ptIndex = 0;\n" + " }\n" + " *dotOut = maxDot;\n" + " return ptIndex;\n" + "}\n" + "#endif //B3_FLOAT4_H\n" + "typedef struct b3Contact4Data b3Contact4Data_t;\n" + "struct b3Contact4Data\n" + "{\n" + " b3Float4 m_worldPosB[4];\n" + "// b3Float4 m_localPosA[4];\n" + "// b3Float4 m_localPosB[4];\n" + " b3Float4 m_worldNormalOnB; // w: m_nPoints\n" + " unsigned short m_restituitionCoeffCmp;\n" + " unsigned short m_frictionCoeffCmp;\n" + " int m_batchIdx;\n" + " int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" + " int m_bodyBPtrAndSignBit;\n" + " int m_childIndexA;\n" + " int m_childIndexB;\n" + " int m_unused1;\n" + " int m_unused2;\n" + "};\n" + "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" + "{\n" + " return (int)contact->m_worldNormalOnB.w;\n" + "};\n" + "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" + "{\n" + " contact->m_worldNormalOnB.w = (float)numPoints;\n" + "};\n" + "#endif //B3_CONTACT4DATA_H\n" + "#ifndef B3_CONTACT_CONSTRAINT5_H\n" + "#define B3_CONTACT_CONSTRAINT5_H\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "typedef struct b3ContactConstraint4 b3ContactConstraint4_t;\n" + "struct b3ContactConstraint4\n" + "{\n" + " b3Float4 m_linear;//normal?\n" + " b3Float4 m_worldPos[4];\n" + " b3Float4 m_center; // friction\n" + " float m_jacCoeffInv[4];\n" + " float m_b[4];\n" + " float m_appliedRambdaDt[4];\n" + " float m_fJacCoeffInv[2]; // friction\n" + " float m_fAppliedRambdaDt[2]; // friction\n" + " unsigned int m_bodyA;\n" + " unsigned int m_bodyB;\n" + " int m_batchIdx;\n" + " unsigned int m_paddings;\n" + "};\n" + "//inline void setFrictionCoeff(float value) { m_linear[3] = value; }\n" + "inline float b3GetFrictionCoeff(b3ContactConstraint4_t* constraint) \n" + "{\n" + " return constraint->m_linear.w; \n" + "}\n" + "#endif //B3_CONTACT_CONSTRAINT5_H\n" + "#ifndef B3_RIGIDBODY_DATA_H\n" + "#define B3_RIGIDBODY_DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_QUAT_H\n" + "#define B3_QUAT_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif\n" + "#endif\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Quat;\n" + " #define b3QuatConstArg const b3Quat\n" + " \n" + " \n" + "inline float4 b3FastNormalize4(float4 v)\n" + "{\n" + " v = (float4)(v.xyz,0.f);\n" + " return fast_normalize(v);\n" + "}\n" + " \n" + "inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" + "inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" + "inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" + "inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" + "inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" + "inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" + "{\n" + " b3Quat ans;\n" + " ans = b3Cross3( a, b );\n" + " ans += a.w*b+b.w*a;\n" + "// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" + " ans.w = a.w*b.w - b3Dot3F4(a, b);\n" + " return ans;\n" + "}\n" + "inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" + "{\n" + " b3Quat q;\n" + " q=in;\n" + " //return b3FastNormalize4(in);\n" + " float len = native_sqrt(dot(q, q));\n" + " if(len > 0.f)\n" + " {\n" + " q *= 1.f / len;\n" + " }\n" + " else\n" + " {\n" + " q.x = q.y = q.z = 0.f;\n" + " q.w = 1.f;\n" + " }\n" + " return q;\n" + "}\n" + "inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" + "{\n" + " b3Quat qInv = b3QuatInvert( q );\n" + " float4 vcpy = vec;\n" + " vcpy.w = 0.f;\n" + " float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" + " return out;\n" + "}\n" + "inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" + "{\n" + " return (b3Quat)(-q.xyz, q.w);\n" + "}\n" + "inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" + "{\n" + " return (b3Quat)(-q.xyz, q.w);\n" + "}\n" + "inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" + "{\n" + " return b3QuatRotate( b3QuatInvert( q ), vec );\n" + "}\n" + "inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" + "{\n" + " return b3QuatRotate( orientation, point ) + (translation);\n" + "}\n" + " \n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "#ifndef B3_MAT3x3_H\n" + "#define B3_MAT3x3_H\n" + "#ifndef B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "typedef struct\n" + "{\n" + " b3Float4 m_row[3];\n" + "}b3Mat3x3;\n" + "#define b3Mat3x3ConstArg const b3Mat3x3\n" + "#define b3GetRow(m,row) (m.m_row[row])\n" + "inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" + "{\n" + " b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" + " b3Mat3x3 out;\n" + " out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" + " out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" + " out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" + " out.m_row[0].w = 0.f;\n" + " out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" + " out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" + " out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" + " out.m_row[1].w = 0.f;\n" + " out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" + " out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" + " out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" + " out.m_row[2].w = 0.f;\n" + " return out;\n" + "}\n" + "inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" + "{\n" + " b3Mat3x3 out;\n" + " out.m_row[0] = fabs(matIn.m_row[0]);\n" + " out.m_row[1] = fabs(matIn.m_row[1]);\n" + " out.m_row[2] = fabs(matIn.m_row[2]);\n" + " return out;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtZero();\n" + "__inline\n" + "b3Mat3x3 mtIdentity();\n" + "__inline\n" + "b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" + "__inline\n" + "b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" + "__inline\n" + "b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" + "__inline\n" + "b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" + "__inline\n" + "b3Mat3x3 mtZero()\n" + "{\n" + " b3Mat3x3 m;\n" + " m.m_row[0] = (b3Float4)(0.f);\n" + " m.m_row[1] = (b3Float4)(0.f);\n" + " m.m_row[2] = (b3Float4)(0.f);\n" + " return m;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtIdentity()\n" + "{\n" + " b3Mat3x3 m;\n" + " m.m_row[0] = (b3Float4)(1,0,0,0);\n" + " m.m_row[1] = (b3Float4)(0,1,0,0);\n" + " m.m_row[2] = (b3Float4)(0,0,1,0);\n" + " return m;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" + "{\n" + " b3Mat3x3 out;\n" + " out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" + " out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" + " out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" + " return out;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" + "{\n" + " b3Mat3x3 transB;\n" + " transB = mtTranspose( b );\n" + " b3Mat3x3 ans;\n" + " // why this doesn't run when 0ing in the for{}\n" + " a.m_row[0].w = 0.f;\n" + " a.m_row[1].w = 0.f;\n" + " a.m_row[2].w = 0.f;\n" + " for(int i=0; i<3; i++)\n" + " {\n" + "// a.m_row[i].w = 0.f;\n" + " ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" + " ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" + " ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" + " ans.m_row[i].w = 0.f;\n" + " }\n" + " return ans;\n" + "}\n" + "__inline\n" + "b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" + "{\n" + " b3Float4 ans;\n" + " ans.x = b3Dot3F4( a.m_row[0], b );\n" + " ans.y = b3Dot3F4( a.m_row[1], b );\n" + " ans.z = b3Dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" + "{\n" + " b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " b3Float4 ans;\n" + " ans.x = b3Dot3F4( a, colx );\n" + " ans.y = b3Dot3F4( a, coly );\n" + " ans.z = b3Dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "#endif\n" + "#endif //B3_MAT3x3_H\n" + "typedef struct b3RigidBodyData b3RigidBodyData_t;\n" + "struct b3RigidBodyData\n" + "{\n" + " b3Float4 m_pos;\n" + " b3Quat m_quat;\n" + " b3Float4 m_linVel;\n" + " b3Float4 m_angVel;\n" + " int m_collidableIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "};\n" + "typedef struct b3InertiaData b3InertiaData_t;\n" + "struct b3InertiaData\n" + "{\n" + " b3Mat3x3 m_invInertiaWorld;\n" + " b3Mat3x3 m_initInvInertia;\n" + "};\n" + "#endif //B3_RIGIDBODY_DATA_H\n" + " \n" + "void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);\n" + " void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q)\n" + "{\n" + " if (b3Fabs(n.z) > 0.70710678f) {\n" + " // choose p in y-z plane\n" + " float a = n.y*n.y + n.z*n.z;\n" + " float k = 1.f/sqrt(a);\n" + " p[0].x = 0;\n" + " p[0].y = -n.z*k;\n" + " p[0].z = n.y*k;\n" + " // set q = n x p\n" + " q[0].x = a*k;\n" + " q[0].y = -n.x*p[0].z;\n" + " q[0].z = n.x*p[0].y;\n" + " }\n" + " else {\n" + " // choose p in x-y plane\n" + " float a = n.x*n.x + n.y*n.y;\n" + " float k = 1.f/sqrt(a);\n" + " p[0].x = -n.y*k;\n" + " p[0].y = n.x*k;\n" + " p[0].z = 0;\n" + " // set q = n x p\n" + " q[0].x = -n.z*p[0].y;\n" + " q[0].y = n.z*p[0].x;\n" + " q[0].z = a*k;\n" + " }\n" + "}\n" + " \n" + "void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)\n" + "{\n" + " *linear = b3MakeFloat4(n.x,n.y,n.z,0.f);\n" + " *angular0 = b3Cross3(r0, n);\n" + " *angular1 = -b3Cross3(r1, n);\n" + "}\n" + "float calcRelVel( b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,\n" + " b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1 )\n" + "{\n" + " return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);\n" + "}\n" + "float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,\n" + " float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)\n" + "{\n" + " // linear0,1 are normlized\n" + " float jmj0 = invMass0;//b3Dot3F4(linear0, linear0)*invMass0;\n" + " float jmj1 = b3Dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" + " float jmj2 = invMass1;//b3Dot3F4(linear1, linear1)*invMass1;\n" + " float jmj3 = b3Dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" + " return -1.f/(jmj0+jmj1+jmj2+jmj3);\n" + "}\n" + "void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,\n" + " b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB, \n" + " __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,\n" + " b3ContactConstraint4_t* dstC )\n" + "{\n" + " dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);\n" + " dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n" + " float dtInv = 1.f/dt;\n" + " for(int ic=0; ic<4; ic++)\n" + " {\n" + " dstC->m_appliedRambdaDt[ic] = 0.f;\n" + " }\n" + " dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n" + " dstC->m_linear = src->m_worldNormalOnB;\n" + " dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n" + " for(int ic=0; ic<4; ic++)\n" + " {\n" + " b3Float4 r0 = src->m_worldPosB[ic] - posA;\n" + " b3Float4 r1 = src->m_worldPosB[ic] - posB;\n" + " if( ic >= src->m_worldNormalOnB.w )//npoints\n" + " {\n" + " dstC->m_jacCoeffInv[ic] = 0.f;\n" + " continue;\n" + " }\n" + " float relVelN;\n" + " {\n" + " b3Float4 linear, angular0, angular1;\n" + " setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);\n" + " dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n" + " invMassA, &invInertiaA, invMassB, &invInertiaB );\n" + " relVelN = calcRelVel(linear, -linear, angular0, angular1,\n" + " linVelA, angVelA, linVelB, angVelB);\n" + " float e = 0.f;//src->getRestituitionCoeff();\n" + " if( relVelN*relVelN < 0.004f ) e = 0.f;\n" + " dstC->m_b[ic] = e*relVelN;\n" + " //float penetration = src->m_worldPosB[ic].w;\n" + " dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n" + " dstC->m_appliedRambdaDt[ic] = 0.f;\n" + " }\n" + " }\n" + " if( src->m_worldNormalOnB.w > 0 )//npoints\n" + " { // prepare friction\n" + " b3Float4 center = b3MakeFloat4(0.f,0.f,0.f,0.f);\n" + " for(int i=0; i<src->m_worldNormalOnB.w; i++) \n" + " center += src->m_worldPosB[i];\n" + " center /= (float)src->m_worldNormalOnB.w;\n" + " b3Float4 tangent[2];\n" + " b3PlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);\n" + " \n" + " b3Float4 r[2];\n" + " r[0] = center - posA;\n" + " r[1] = center - posB;\n" + " for(int i=0; i<2; i++)\n" + " {\n" + " b3Float4 linear, angular0, angular1;\n" + " setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n" + " dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n" + " invMassA, &invInertiaA, invMassB, &invInertiaB );\n" + " dstC->m_fAppliedRambdaDt[i] = 0.f;\n" + " }\n" + " dstC->m_center = center;\n" + " }\n" + " for(int i=0; i<4; i++)\n" + " {\n" + " if( i<src->m_worldNormalOnB.w )\n" + " {\n" + " dstC->m_worldPos[i] = src->m_worldPosB[i];\n" + " }\n" + " else\n" + " {\n" + " dstC->m_worldPos[i] = b3MakeFloat4(0.f,0.f,0.f,0.f);\n" + " }\n" + " }\n" + "}\n" + "#pragma OPENCL EXTENSION cl_amd_printf : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" + "#ifdef cl_ext_atomic_counters_32\n" + "#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" + "#else\n" + "#define counter32_t volatile global int*\n" + "#endif\n" + "typedef unsigned int u32;\n" + "typedef unsigned short u16;\n" + "typedef unsigned char u8;\n" + "#define GET_GROUP_IDX get_group_id(0)\n" + "#define GET_LOCAL_IDX get_local_id(0)\n" + "#define GET_GLOBAL_IDX get_global_id(0)\n" + "#define GET_GROUP_SIZE get_local_size(0)\n" + "#define GET_NUM_GROUPS get_num_groups(0)\n" + "#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" + "#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" + "#define AtomInc(x) atom_inc(&(x))\n" + "#define AtomInc1(x, out) out = atom_inc(&(x))\n" + "#define AppendInc(x, out) out = atomic_inc(x)\n" + "#define AtomAdd(x, value) atom_add(&(x), value)\n" + "#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" + "#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" + "#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" + "#define make_float4 (float4)\n" + "#define make_float2 (float2)\n" + "#define make_uint4 (uint4)\n" + "#define make_int4 (int4)\n" + "#define make_uint2 (uint2)\n" + "#define make_int2 (int2)\n" + "#define max2 max\n" + "#define min2 min\n" + "///////////////////////////////////////\n" + "// Vector\n" + "///////////////////////////////////////\n" + "__inline\n" + "float fastDiv(float numerator, float denominator)\n" + "{\n" + " return native_divide(numerator, denominator); \n" + "// return numerator/denominator; \n" + "}\n" + "__inline\n" + "float4 fastDiv4(float4 numerator, float4 denominator)\n" + "{\n" + " return native_divide(numerator, denominator); \n" + "}\n" + "__inline\n" + "float fastSqrtf(float f2)\n" + "{\n" + " return native_sqrt(f2);\n" + "// return sqrt(f2);\n" + "}\n" + "__inline\n" + "float fastRSqrt(float f2)\n" + "{\n" + " return native_rsqrt(f2);\n" + "}\n" + "__inline\n" + "float fastLength4(float4 v)\n" + "{\n" + " return fast_length(v);\n" + "}\n" + "__inline\n" + "float4 fastNormalize4(float4 v)\n" + "{\n" + " return fast_normalize(v);\n" + "}\n" + "__inline\n" + "float sqrtf(float a)\n" + "{\n" + "// return sqrt(a);\n" + " return native_sqrt(a);\n" + "}\n" + "__inline\n" + "float4 cross3(float4 a, float4 b)\n" + "{\n" + " return cross(a,b);\n" + "}\n" + "__inline\n" + "float dot3F4(float4 a, float4 b)\n" + "{\n" + " float4 a1 = make_float4(a.xyz,0.f);\n" + " float4 b1 = make_float4(b.xyz,0.f);\n" + " return dot(a1, b1);\n" + "}\n" + "__inline\n" + "float length3(const float4 a)\n" + "{\n" + " return sqrtf(dot3F4(a,a));\n" + "}\n" + "__inline\n" + "float dot4(const float4 a, const float4 b)\n" + "{\n" + " return dot( a, b );\n" + "}\n" + "// for height\n" + "__inline\n" + "float dot3w1(const float4 point, const float4 eqn)\n" + "{\n" + " return dot3F4(point,eqn) + eqn.w;\n" + "}\n" + "__inline\n" + "float4 normalize3(const float4 a)\n" + "{\n" + " float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" + " return fastNormalize4( n );\n" + "// float length = sqrtf(dot3F4(a, a));\n" + "// return 1.f/length * a;\n" + "}\n" + "__inline\n" + "float4 normalize4(const float4 a)\n" + "{\n" + " float length = sqrtf(dot4(a, a));\n" + " return 1.f/length * a;\n" + "}\n" + "__inline\n" + "float4 createEquation(const float4 a, const float4 b, const float4 c)\n" + "{\n" + " float4 eqn;\n" + " float4 ab = b-a;\n" + " float4 ac = c-a;\n" + " eqn = normalize3( cross3(ab, ac) );\n" + " eqn.w = -dot3F4(eqn,a);\n" + " return eqn;\n" + "}\n" + "#define WG_SIZE 64\n" + "typedef struct\n" + "{\n" + " int m_nConstraints;\n" + " int m_start;\n" + " int m_batchIdx;\n" + " int m_nSplit;\n" + "// int m_paddings[1];\n" + "} ConstBuffer;\n" + "typedef struct\n" + "{\n" + " int m_solveFriction;\n" + " int m_maxBatch; // long batch really kills the performance\n" + " int m_batchIdx;\n" + " int m_nSplit;\n" + "// int m_paddings[1];\n" + "} ConstBufferBatchSolve;\n" + " \n" + "typedef struct \n" + "{\n" + " int m_valInt0;\n" + " int m_valInt1;\n" + " int m_valInt2;\n" + " int m_valInt3;\n" + " float m_val0;\n" + " float m_val1;\n" + " float m_val2;\n" + " float m_val3;\n" + "} SolverDebugInfo;\n" + "typedef struct\n" + "{\n" + " int m_nContacts;\n" + " float m_dt;\n" + " float m_positionDrift;\n" + " float m_positionConstraintCoeff;\n" + "} ConstBufferCTC;\n" + "__kernel\n" + "__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void ContactToConstraintKernel(__global struct b3Contact4Data* gContact, __global b3RigidBodyData_t* gBodies, __global b3InertiaData_t* gShapes, __global b3ContactConstraint4_t* gConstraintOut, \n" + "int nContacts,\n" + "float dt,\n" + "float positionDrift,\n" + "float positionConstraintCoeff\n" + ")\n" + "{\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " \n" + " if( gIdx < nContacts )\n" + " {\n" + " int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n" + " int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n" + " float4 posA = gBodies[aIdx].m_pos;\n" + " float4 linVelA = gBodies[aIdx].m_linVel;\n" + " float4 angVelA = gBodies[aIdx].m_angVel;\n" + " float invMassA = gBodies[aIdx].m_invMass;\n" + " b3Mat3x3 invInertiaA = gShapes[aIdx].m_initInvInertia;\n" + " float4 posB = gBodies[bIdx].m_pos;\n" + " float4 linVelB = gBodies[bIdx].m_linVel;\n" + " float4 angVelB = gBodies[bIdx].m_angVel;\n" + " float invMassB = gBodies[bIdx].m_invMass;\n" + " b3Mat3x3 invInertiaB = gShapes[bIdx].m_initInvInertia;\n" + " b3ContactConstraint4_t cs;\n" + " setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n" + " &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,\n" + " &cs );\n" + " \n" + " cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n" + " gConstraintOut[gIdx] = cs;\n" + " }\n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h index 1b5819f6cf..1e6e3579b6 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h @@ -1,601 +1,600 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* solverSetup2CL= \ -"/*\n" -"Copyright (c) 2012 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Takahiro Harada\n" -"#ifndef B3_CONTACT4DATA_H\n" -"#define B3_CONTACT4DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#define B3_FLOAT4_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#define B3_PLATFORM_DEFINITIONS_H\n" -"struct MyTest\n" -"{\n" -" int bla;\n" -"};\n" -"#ifdef __cplusplus\n" -"#else\n" -"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" -"#define B3_LARGE_FLOAT 1e18f\n" -"#define B3_INFINITY 1e18f\n" -"#define b3Assert(a)\n" -"#define b3ConstArray(a) __global const a*\n" -"#define b3AtomicInc atomic_inc\n" -"#define b3AtomicAdd atomic_add\n" -"#define b3Fabs fabs\n" -"#define b3Sqrt native_sqrt\n" -"#define b3Sin native_sin\n" -"#define b3Cos native_cos\n" -"#define B3_STATIC\n" -"#endif\n" -"#endif\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Float4;\n" -" #define b3Float4ConstArg const b3Float4\n" -" #define b3MakeFloat4 (float4)\n" -" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return dot(a1, b1);\n" -" }\n" -" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return cross(a1, b1);\n" -" }\n" -" #define b3MinFloat4 min\n" -" #define b3MaxFloat4 max\n" -" #define b3Normalized(a) normalize(a)\n" -"#endif \n" -" \n" -"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" -"{\n" -" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" -" return false;\n" -" return true;\n" -"}\n" -"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" -"{\n" -" float maxDot = -B3_INFINITY;\n" -" int i = 0;\n" -" int ptIndex = -1;\n" -" for( i = 0; i < vecLen; i++ )\n" -" {\n" -" float dot = b3Dot3F4(vecArray[i],vec);\n" -" \n" -" if( dot > maxDot )\n" -" {\n" -" maxDot = dot;\n" -" ptIndex = i;\n" -" }\n" -" }\n" -" b3Assert(ptIndex>=0);\n" -" if (ptIndex<0)\n" -" {\n" -" ptIndex = 0;\n" -" }\n" -" *dotOut = maxDot;\n" -" return ptIndex;\n" -"}\n" -"#endif //B3_FLOAT4_H\n" -"typedef struct b3Contact4Data b3Contact4Data_t;\n" -"struct b3Contact4Data\n" -"{\n" -" b3Float4 m_worldPosB[4];\n" -"// b3Float4 m_localPosA[4];\n" -"// b3Float4 m_localPosB[4];\n" -" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" -" unsigned short m_restituitionCoeffCmp;\n" -" unsigned short m_frictionCoeffCmp;\n" -" int m_batchIdx;\n" -" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" -" int m_bodyBPtrAndSignBit;\n" -" int m_childIndexA;\n" -" int m_childIndexB;\n" -" int m_unused1;\n" -" int m_unused2;\n" -"};\n" -"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" -"{\n" -" return (int)contact->m_worldNormalOnB.w;\n" -"};\n" -"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" -"{\n" -" contact->m_worldNormalOnB.w = (float)numPoints;\n" -"};\n" -"#endif //B3_CONTACT4DATA_H\n" -"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" -"#ifdef cl_ext_atomic_counters_32\n" -"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" -"#else\n" -"#define counter32_t volatile global int*\n" -"#endif\n" -"typedef unsigned int u32;\n" -"typedef unsigned short u16;\n" -"typedef unsigned char u8;\n" -"#define GET_GROUP_IDX get_group_id(0)\n" -"#define GET_LOCAL_IDX get_local_id(0)\n" -"#define GET_GLOBAL_IDX get_global_id(0)\n" -"#define GET_GROUP_SIZE get_local_size(0)\n" -"#define GET_NUM_GROUPS get_num_groups(0)\n" -"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" -"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" -"#define AtomInc(x) atom_inc(&(x))\n" -"#define AtomInc1(x, out) out = atom_inc(&(x))\n" -"#define AppendInc(x, out) out = atomic_inc(x)\n" -"#define AtomAdd(x, value) atom_add(&(x), value)\n" -"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" -"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" -"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" -"#define make_float4 (float4)\n" -"#define make_float2 (float2)\n" -"#define make_uint4 (uint4)\n" -"#define make_int4 (int4)\n" -"#define make_uint2 (uint2)\n" -"#define make_int2 (int2)\n" -"#define max2 max\n" -"#define min2 min\n" -"///////////////////////////////////////\n" -"// Vector\n" -"///////////////////////////////////////\n" -"__inline\n" -"float fastDiv(float numerator, float denominator)\n" -"{\n" -" return native_divide(numerator, denominator); \n" -"// return numerator/denominator; \n" -"}\n" -"__inline\n" -"float4 fastDiv4(float4 numerator, float4 denominator)\n" -"{\n" -" return native_divide(numerator, denominator); \n" -"}\n" -"__inline\n" -"float fastSqrtf(float f2)\n" -"{\n" -" return native_sqrt(f2);\n" -"// return sqrt(f2);\n" -"}\n" -"__inline\n" -"float fastRSqrt(float f2)\n" -"{\n" -" return native_rsqrt(f2);\n" -"}\n" -"__inline\n" -"float fastLength4(float4 v)\n" -"{\n" -" return fast_length(v);\n" -"}\n" -"__inline\n" -"float4 fastNormalize4(float4 v)\n" -"{\n" -" return fast_normalize(v);\n" -"}\n" -"__inline\n" -"float sqrtf(float a)\n" -"{\n" -"// return sqrt(a);\n" -" return native_sqrt(a);\n" -"}\n" -"__inline\n" -"float4 cross3(float4 a, float4 b)\n" -"{\n" -" return cross(a,b);\n" -"}\n" -"__inline\n" -"float dot3F4(float4 a, float4 b)\n" -"{\n" -" float4 a1 = make_float4(a.xyz,0.f);\n" -" float4 b1 = make_float4(b.xyz,0.f);\n" -" return dot(a1, b1);\n" -"}\n" -"__inline\n" -"float length3(const float4 a)\n" -"{\n" -" return sqrtf(dot3F4(a,a));\n" -"}\n" -"__inline\n" -"float dot4(const float4 a, const float4 b)\n" -"{\n" -" return dot( a, b );\n" -"}\n" -"// for height\n" -"__inline\n" -"float dot3w1(const float4 point, const float4 eqn)\n" -"{\n" -" return dot3F4(point,eqn) + eqn.w;\n" -"}\n" -"__inline\n" -"float4 normalize3(const float4 a)\n" -"{\n" -" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" -" return fastNormalize4( n );\n" -"// float length = sqrtf(dot3F4(a, a));\n" -"// return 1.f/length * a;\n" -"}\n" -"__inline\n" -"float4 normalize4(const float4 a)\n" -"{\n" -" float length = sqrtf(dot4(a, a));\n" -" return 1.f/length * a;\n" -"}\n" -"__inline\n" -"float4 createEquation(const float4 a, const float4 b, const float4 c)\n" -"{\n" -" float4 eqn;\n" -" float4 ab = b-a;\n" -" float4 ac = c-a;\n" -" eqn = normalize3( cross3(ab, ac) );\n" -" eqn.w = -dot3F4(eqn,a);\n" -" return eqn;\n" -"}\n" -"///////////////////////////////////////\n" -"// Matrix3x3\n" -"///////////////////////////////////////\n" -"typedef struct\n" -"{\n" -" float4 m_row[3];\n" -"}Matrix3x3;\n" -"__inline\n" -"Matrix3x3 mtZero();\n" -"__inline\n" -"Matrix3x3 mtIdentity();\n" -"__inline\n" -"Matrix3x3 mtTranspose(Matrix3x3 m);\n" -"__inline\n" -"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b);\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b);\n" -"__inline\n" -"Matrix3x3 mtZero()\n" -"{\n" -" Matrix3x3 m;\n" -" m.m_row[0] = (float4)(0.f);\n" -" m.m_row[1] = (float4)(0.f);\n" -" m.m_row[2] = (float4)(0.f);\n" -" return m;\n" -"}\n" -"__inline\n" -"Matrix3x3 mtIdentity()\n" -"{\n" -" Matrix3x3 m;\n" -" m.m_row[0] = (float4)(1,0,0,0);\n" -" m.m_row[1] = (float4)(0,1,0,0);\n" -" m.m_row[2] = (float4)(0,0,1,0);\n" -" return m;\n" -"}\n" -"__inline\n" -"Matrix3x3 mtTranspose(Matrix3x3 m)\n" -"{\n" -" Matrix3x3 out;\n" -" out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" -" out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" -" out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" -" return out;\n" -"}\n" -"__inline\n" -"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n" -"{\n" -" Matrix3x3 transB;\n" -" transB = mtTranspose( b );\n" -" Matrix3x3 ans;\n" -" // why this doesn't run when 0ing in the for{}\n" -" a.m_row[0].w = 0.f;\n" -" a.m_row[1].w = 0.f;\n" -" a.m_row[2].w = 0.f;\n" -" for(int i=0; i<3; i++)\n" -" {\n" -"// a.m_row[i].w = 0.f;\n" -" ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n" -" ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n" -" ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n" -" ans.m_row[i].w = 0.f;\n" -" }\n" -" return ans;\n" -"}\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b)\n" -"{\n" -" float4 ans;\n" -" ans.x = dot3F4( a.m_row[0], b );\n" -" ans.y = dot3F4( a.m_row[1], b );\n" -" ans.z = dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b)\n" -"{\n" -" float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" float4 ans;\n" -" ans.x = dot3F4( a, colx );\n" -" ans.y = dot3F4( a, coly );\n" -" ans.z = dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"///////////////////////////////////////\n" -"// Quaternion\n" -"///////////////////////////////////////\n" -"typedef float4 Quaternion;\n" -"__inline\n" -"Quaternion qtMul(Quaternion a, Quaternion b);\n" -"__inline\n" -"Quaternion qtNormalize(Quaternion in);\n" -"__inline\n" -"float4 qtRotate(Quaternion q, float4 vec);\n" -"__inline\n" -"Quaternion qtInvert(Quaternion q);\n" -"__inline\n" -"Quaternion qtMul(Quaternion a, Quaternion b)\n" -"{\n" -" Quaternion ans;\n" -" ans = cross3( a, b );\n" -" ans += a.w*b+b.w*a;\n" -"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" -" ans.w = a.w*b.w - dot3F4(a, b);\n" -" return ans;\n" -"}\n" -"__inline\n" -"Quaternion qtNormalize(Quaternion in)\n" -"{\n" -" return fastNormalize4(in);\n" -"// in /= length( in );\n" -"// return in;\n" -"}\n" -"__inline\n" -"float4 qtRotate(Quaternion q, float4 vec)\n" -"{\n" -" Quaternion qInv = qtInvert( q );\n" -" float4 vcpy = vec;\n" -" vcpy.w = 0.f;\n" -" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" -" return out;\n" -"}\n" -"__inline\n" -"Quaternion qtInvert(Quaternion q)\n" -"{\n" -" return (Quaternion)(-q.xyz, q.w);\n" -"}\n" -"__inline\n" -"float4 qtInvRotate(const Quaternion q, float4 vec)\n" -"{\n" -" return qtRotate( qtInvert( q ), vec );\n" -"}\n" -"#define WG_SIZE 64\n" -"typedef struct\n" -"{\n" -" float4 m_pos;\n" -" Quaternion m_quat;\n" -" float4 m_linVel;\n" -" float4 m_angVel;\n" -" u32 m_shapeIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"} Body;\n" -"typedef struct\n" -"{\n" -" Matrix3x3 m_invInertia;\n" -" Matrix3x3 m_initInvInertia;\n" -"} Shape;\n" -"typedef struct\n" -"{\n" -" float4 m_linear;\n" -" float4 m_worldPos[4];\n" -" float4 m_center; \n" -" float m_jacCoeffInv[4];\n" -" float m_b[4];\n" -" float m_appliedRambdaDt[4];\n" -" float m_fJacCoeffInv[2]; \n" -" float m_fAppliedRambdaDt[2]; \n" -" u32 m_bodyA;\n" -" u32 m_bodyB;\n" -" int m_batchIdx;\n" -" u32 m_paddings[1];\n" -"} Constraint4;\n" -"typedef struct\n" -"{\n" -" int m_nConstraints;\n" -" int m_start;\n" -" int m_batchIdx;\n" -" int m_nSplit;\n" -"// int m_paddings[1];\n" -"} ConstBuffer;\n" -"typedef struct\n" -"{\n" -" int m_solveFriction;\n" -" int m_maxBatch; // long batch really kills the performance\n" -" int m_batchIdx;\n" -" int m_nSplit;\n" -"// int m_paddings[1];\n" -"} ConstBufferBatchSolve;\n" -" \n" -"typedef struct \n" -"{\n" -" int m_valInt0;\n" -" int m_valInt1;\n" -" int m_valInt2;\n" -" int m_valInt3;\n" -" float m_val0;\n" -" float m_val1;\n" -" float m_val2;\n" -" float m_val3;\n" -"} SolverDebugInfo;\n" -"// others\n" -"__kernel\n" -"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void ReorderContactKernel(__global struct b3Contact4Data* in, __global struct b3Contact4Data* out, __global int2* sortData, int4 cb )\n" -"{\n" -" int nContacts = cb.x;\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" if( gIdx < nContacts )\n" -" {\n" -" int srcIdx = sortData[gIdx].y;\n" -" out[gIdx] = in[srcIdx];\n" -" }\n" -"}\n" -"__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void SetDeterminismSortDataChildShapeB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataOut, int nContacts)\n" -"{\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" if( gIdx < nContacts )\n" -" {\n" -" int2 sd;\n" -" sd.x = contactsIn[gIdx].m_childIndexB;\n" -" sd.y = gIdx;\n" -" sortDataOut[gIdx] = sd;\n" -" }\n" -"}\n" -"__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void SetDeterminismSortDataChildShapeA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" -"{\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" if( gIdx < nContacts )\n" -" {\n" -" int2 sdIn;\n" -" sdIn = sortDataInOut[gIdx];\n" -" int2 sdOut;\n" -" sdOut.x = contactsIn[sdIn.y].m_childIndexA;\n" -" sdOut.y = sdIn.y;\n" -" sortDataInOut[gIdx] = sdOut;\n" -" }\n" -"}\n" -"__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void SetDeterminismSortDataBodyA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" -"{\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" if( gIdx < nContacts )\n" -" {\n" -" int2 sdIn;\n" -" sdIn = sortDataInOut[gIdx];\n" -" int2 sdOut;\n" -" sdOut.x = contactsIn[sdIn.y].m_bodyAPtrAndSignBit;\n" -" sdOut.y = sdIn.y;\n" -" sortDataInOut[gIdx] = sdOut;\n" -" }\n" -"}\n" -"__kernel\n" -"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void SetDeterminismSortDataBodyB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" -"{\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" if( gIdx < nContacts )\n" -" {\n" -" int2 sdIn;\n" -" sdIn = sortDataInOut[gIdx];\n" -" int2 sdOut;\n" -" sdOut.x = contactsIn[sdIn.y].m_bodyBPtrAndSignBit;\n" -" sdOut.y = sdIn.y;\n" -" sortDataInOut[gIdx] = sdOut;\n" -" }\n" -"}\n" -"typedef struct\n" -"{\n" -" int m_nContacts;\n" -" int m_staticIdx;\n" -" float m_scale;\n" -" int m_nSplit;\n" -"} ConstBufferSSD;\n" -"__constant const int gridTable4x4[] = \n" -"{\n" -" 0,1,17,16,\n" -" 1,2,18,19,\n" -" 17,18,32,3,\n" -" 16,19,3,34\n" -"};\n" -"__constant const int gridTable8x8[] = \n" -"{\n" -" 0, 2, 3, 16, 17, 18, 19, 1,\n" -" 66, 64, 80, 67, 82, 81, 65, 83,\n" -" 131,144,128,130,147,129,145,146,\n" -" 208,195,194,192,193,211,210,209,\n" -" 21, 22, 23, 5, 4, 6, 7, 20,\n" -" 86, 85, 69, 87, 70, 68, 84, 71,\n" -" 151,133,149,150,135,148,132,134,\n" -" 197,27,214,213,212,199,198,196\n" -" \n" -"};\n" -"#define USE_SPATIAL_BATCHING 1\n" -"#define USE_4x4_GRID 1\n" -"__kernel\n" -"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void SetSortDataKernel(__global struct b3Contact4Data* gContact, __global Body* gBodies, __global int2* gSortDataOut, \n" -"int nContacts,float scale,int4 nSplit,int staticIdx)\n" -"{\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" \n" -" if( gIdx < nContacts )\n" -" {\n" -" int aPtrAndSignBit = gContact[gIdx].m_bodyAPtrAndSignBit;\n" -" int bPtrAndSignBit = gContact[gIdx].m_bodyBPtrAndSignBit;\n" -" int aIdx = abs(aPtrAndSignBit );\n" -" int bIdx = abs(bPtrAndSignBit);\n" -" bool aStatic = (aPtrAndSignBit<0) ||(aPtrAndSignBit==staticIdx);\n" -" bool bStatic = (bPtrAndSignBit<0) ||(bPtrAndSignBit==staticIdx);\n" -"#if USE_SPATIAL_BATCHING \n" -" int idx = (aStatic)? bIdx: aIdx;\n" -" float4 p = gBodies[idx].m_pos;\n" -" int xIdx = (int)((p.x-((p.x<0.f)?1.f:0.f))*scale) & (nSplit.x-1);\n" -" int yIdx = (int)((p.y-((p.y<0.f)?1.f:0.f))*scale) & (nSplit.y-1);\n" -" int zIdx = (int)((p.z-((p.z<0.f)?1.f:0.f))*scale) & (nSplit.z-1);\n" -" int newIndex = (xIdx+yIdx*nSplit.x+zIdx*nSplit.x*nSplit.y);\n" -" \n" -"#else//USE_SPATIAL_BATCHING\n" -" #if USE_4x4_GRID\n" -" int aa = aIdx&3;\n" -" int bb = bIdx&3;\n" -" if (aStatic)\n" -" aa = bb;\n" -" if (bStatic)\n" -" bb = aa;\n" -" int gridIndex = aa + bb*4;\n" -" int newIndex = gridTable4x4[gridIndex];\n" -" #else//USE_4x4_GRID\n" -" int aa = aIdx&7;\n" -" int bb = bIdx&7;\n" -" if (aStatic)\n" -" aa = bb;\n" -" if (bStatic)\n" -" bb = aa;\n" -" int gridIndex = aa + bb*8;\n" -" int newIndex = gridTable8x8[gridIndex];\n" -" #endif//USE_4x4_GRID\n" -"#endif//USE_SPATIAL_BATCHING\n" -" gSortDataOut[gIdx].x = newIndex;\n" -" gSortDataOut[gIdx].y = gIdx;\n" -" }\n" -" else\n" -" {\n" -" gSortDataOut[gIdx].x = 0xffffffff;\n" -" }\n" -"}\n" -"__kernel\n" -"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void CopyConstraintKernel(__global struct b3Contact4Data* gIn, __global struct b3Contact4Data* gOut, int4 cb )\n" -"{\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" if( gIdx < cb.x )\n" -" {\n" -" gOut[gIdx] = gIn[gIdx];\n" -" }\n" -"}\n" -; +static const char* solverSetup2CL = + "/*\n" + "Copyright (c) 2012 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Takahiro Harada\n" + "#ifndef B3_CONTACT4DATA_H\n" + "#define B3_CONTACT4DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#define B3_FLOAT4_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#define B3_PLATFORM_DEFINITIONS_H\n" + "struct MyTest\n" + "{\n" + " int bla;\n" + "};\n" + "#ifdef __cplusplus\n" + "#else\n" + "//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" + "#define B3_LARGE_FLOAT 1e18f\n" + "#define B3_INFINITY 1e18f\n" + "#define b3Assert(a)\n" + "#define b3ConstArray(a) __global const a*\n" + "#define b3AtomicInc atomic_inc\n" + "#define b3AtomicAdd atomic_add\n" + "#define b3Fabs fabs\n" + "#define b3Sqrt native_sqrt\n" + "#define b3Sin native_sin\n" + "#define b3Cos native_cos\n" + "#define B3_STATIC\n" + "#endif\n" + "#endif\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Float4;\n" + " #define b3Float4ConstArg const b3Float4\n" + " #define b3MakeFloat4 (float4)\n" + " float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return dot(a1, b1);\n" + " }\n" + " b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return cross(a1, b1);\n" + " }\n" + " #define b3MinFloat4 min\n" + " #define b3MaxFloat4 max\n" + " #define b3Normalized(a) normalize(a)\n" + "#endif \n" + " \n" + "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" + "{\n" + " if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" + " return false;\n" + " return true;\n" + "}\n" + "inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" + "{\n" + " float maxDot = -B3_INFINITY;\n" + " int i = 0;\n" + " int ptIndex = -1;\n" + " for( i = 0; i < vecLen; i++ )\n" + " {\n" + " float dot = b3Dot3F4(vecArray[i],vec);\n" + " \n" + " if( dot > maxDot )\n" + " {\n" + " maxDot = dot;\n" + " ptIndex = i;\n" + " }\n" + " }\n" + " b3Assert(ptIndex>=0);\n" + " if (ptIndex<0)\n" + " {\n" + " ptIndex = 0;\n" + " }\n" + " *dotOut = maxDot;\n" + " return ptIndex;\n" + "}\n" + "#endif //B3_FLOAT4_H\n" + "typedef struct b3Contact4Data b3Contact4Data_t;\n" + "struct b3Contact4Data\n" + "{\n" + " b3Float4 m_worldPosB[4];\n" + "// b3Float4 m_localPosA[4];\n" + "// b3Float4 m_localPosB[4];\n" + " b3Float4 m_worldNormalOnB; // w: m_nPoints\n" + " unsigned short m_restituitionCoeffCmp;\n" + " unsigned short m_frictionCoeffCmp;\n" + " int m_batchIdx;\n" + " int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" + " int m_bodyBPtrAndSignBit;\n" + " int m_childIndexA;\n" + " int m_childIndexB;\n" + " int m_unused1;\n" + " int m_unused2;\n" + "};\n" + "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" + "{\n" + " return (int)contact->m_worldNormalOnB.w;\n" + "};\n" + "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" + "{\n" + " contact->m_worldNormalOnB.w = (float)numPoints;\n" + "};\n" + "#endif //B3_CONTACT4DATA_H\n" + "#pragma OPENCL EXTENSION cl_amd_printf : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" + "#ifdef cl_ext_atomic_counters_32\n" + "#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" + "#else\n" + "#define counter32_t volatile global int*\n" + "#endif\n" + "typedef unsigned int u32;\n" + "typedef unsigned short u16;\n" + "typedef unsigned char u8;\n" + "#define GET_GROUP_IDX get_group_id(0)\n" + "#define GET_LOCAL_IDX get_local_id(0)\n" + "#define GET_GLOBAL_IDX get_global_id(0)\n" + "#define GET_GROUP_SIZE get_local_size(0)\n" + "#define GET_NUM_GROUPS get_num_groups(0)\n" + "#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" + "#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" + "#define AtomInc(x) atom_inc(&(x))\n" + "#define AtomInc1(x, out) out = atom_inc(&(x))\n" + "#define AppendInc(x, out) out = atomic_inc(x)\n" + "#define AtomAdd(x, value) atom_add(&(x), value)\n" + "#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" + "#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" + "#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" + "#define make_float4 (float4)\n" + "#define make_float2 (float2)\n" + "#define make_uint4 (uint4)\n" + "#define make_int4 (int4)\n" + "#define make_uint2 (uint2)\n" + "#define make_int2 (int2)\n" + "#define max2 max\n" + "#define min2 min\n" + "///////////////////////////////////////\n" + "// Vector\n" + "///////////////////////////////////////\n" + "__inline\n" + "float fastDiv(float numerator, float denominator)\n" + "{\n" + " return native_divide(numerator, denominator); \n" + "// return numerator/denominator; \n" + "}\n" + "__inline\n" + "float4 fastDiv4(float4 numerator, float4 denominator)\n" + "{\n" + " return native_divide(numerator, denominator); \n" + "}\n" + "__inline\n" + "float fastSqrtf(float f2)\n" + "{\n" + " return native_sqrt(f2);\n" + "// return sqrt(f2);\n" + "}\n" + "__inline\n" + "float fastRSqrt(float f2)\n" + "{\n" + " return native_rsqrt(f2);\n" + "}\n" + "__inline\n" + "float fastLength4(float4 v)\n" + "{\n" + " return fast_length(v);\n" + "}\n" + "__inline\n" + "float4 fastNormalize4(float4 v)\n" + "{\n" + " return fast_normalize(v);\n" + "}\n" + "__inline\n" + "float sqrtf(float a)\n" + "{\n" + "// return sqrt(a);\n" + " return native_sqrt(a);\n" + "}\n" + "__inline\n" + "float4 cross3(float4 a, float4 b)\n" + "{\n" + " return cross(a,b);\n" + "}\n" + "__inline\n" + "float dot3F4(float4 a, float4 b)\n" + "{\n" + " float4 a1 = make_float4(a.xyz,0.f);\n" + " float4 b1 = make_float4(b.xyz,0.f);\n" + " return dot(a1, b1);\n" + "}\n" + "__inline\n" + "float length3(const float4 a)\n" + "{\n" + " return sqrtf(dot3F4(a,a));\n" + "}\n" + "__inline\n" + "float dot4(const float4 a, const float4 b)\n" + "{\n" + " return dot( a, b );\n" + "}\n" + "// for height\n" + "__inline\n" + "float dot3w1(const float4 point, const float4 eqn)\n" + "{\n" + " return dot3F4(point,eqn) + eqn.w;\n" + "}\n" + "__inline\n" + "float4 normalize3(const float4 a)\n" + "{\n" + " float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" + " return fastNormalize4( n );\n" + "// float length = sqrtf(dot3F4(a, a));\n" + "// return 1.f/length * a;\n" + "}\n" + "__inline\n" + "float4 normalize4(const float4 a)\n" + "{\n" + " float length = sqrtf(dot4(a, a));\n" + " return 1.f/length * a;\n" + "}\n" + "__inline\n" + "float4 createEquation(const float4 a, const float4 b, const float4 c)\n" + "{\n" + " float4 eqn;\n" + " float4 ab = b-a;\n" + " float4 ac = c-a;\n" + " eqn = normalize3( cross3(ab, ac) );\n" + " eqn.w = -dot3F4(eqn,a);\n" + " return eqn;\n" + "}\n" + "///////////////////////////////////////\n" + "// Matrix3x3\n" + "///////////////////////////////////////\n" + "typedef struct\n" + "{\n" + " float4 m_row[3];\n" + "}Matrix3x3;\n" + "__inline\n" + "Matrix3x3 mtZero();\n" + "__inline\n" + "Matrix3x3 mtIdentity();\n" + "__inline\n" + "Matrix3x3 mtTranspose(Matrix3x3 m);\n" + "__inline\n" + "Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b);\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b);\n" + "__inline\n" + "Matrix3x3 mtZero()\n" + "{\n" + " Matrix3x3 m;\n" + " m.m_row[0] = (float4)(0.f);\n" + " m.m_row[1] = (float4)(0.f);\n" + " m.m_row[2] = (float4)(0.f);\n" + " return m;\n" + "}\n" + "__inline\n" + "Matrix3x3 mtIdentity()\n" + "{\n" + " Matrix3x3 m;\n" + " m.m_row[0] = (float4)(1,0,0,0);\n" + " m.m_row[1] = (float4)(0,1,0,0);\n" + " m.m_row[2] = (float4)(0,0,1,0);\n" + " return m;\n" + "}\n" + "__inline\n" + "Matrix3x3 mtTranspose(Matrix3x3 m)\n" + "{\n" + " Matrix3x3 out;\n" + " out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" + " out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" + " out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" + " return out;\n" + "}\n" + "__inline\n" + "Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n" + "{\n" + " Matrix3x3 transB;\n" + " transB = mtTranspose( b );\n" + " Matrix3x3 ans;\n" + " // why this doesn't run when 0ing in the for{}\n" + " a.m_row[0].w = 0.f;\n" + " a.m_row[1].w = 0.f;\n" + " a.m_row[2].w = 0.f;\n" + " for(int i=0; i<3; i++)\n" + " {\n" + "// a.m_row[i].w = 0.f;\n" + " ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n" + " ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n" + " ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n" + " ans.m_row[i].w = 0.f;\n" + " }\n" + " return ans;\n" + "}\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b)\n" + "{\n" + " float4 ans;\n" + " ans.x = dot3F4( a.m_row[0], b );\n" + " ans.y = dot3F4( a.m_row[1], b );\n" + " ans.z = dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b)\n" + "{\n" + " float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " float4 ans;\n" + " ans.x = dot3F4( a, colx );\n" + " ans.y = dot3F4( a, coly );\n" + " ans.z = dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "///////////////////////////////////////\n" + "// Quaternion\n" + "///////////////////////////////////////\n" + "typedef float4 Quaternion;\n" + "__inline\n" + "Quaternion qtMul(Quaternion a, Quaternion b);\n" + "__inline\n" + "Quaternion qtNormalize(Quaternion in);\n" + "__inline\n" + "float4 qtRotate(Quaternion q, float4 vec);\n" + "__inline\n" + "Quaternion qtInvert(Quaternion q);\n" + "__inline\n" + "Quaternion qtMul(Quaternion a, Quaternion b)\n" + "{\n" + " Quaternion ans;\n" + " ans = cross3( a, b );\n" + " ans += a.w*b+b.w*a;\n" + "// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" + " ans.w = a.w*b.w - dot3F4(a, b);\n" + " return ans;\n" + "}\n" + "__inline\n" + "Quaternion qtNormalize(Quaternion in)\n" + "{\n" + " return fastNormalize4(in);\n" + "// in /= length( in );\n" + "// return in;\n" + "}\n" + "__inline\n" + "float4 qtRotate(Quaternion q, float4 vec)\n" + "{\n" + " Quaternion qInv = qtInvert( q );\n" + " float4 vcpy = vec;\n" + " vcpy.w = 0.f;\n" + " float4 out = qtMul(qtMul(q,vcpy),qInv);\n" + " return out;\n" + "}\n" + "__inline\n" + "Quaternion qtInvert(Quaternion q)\n" + "{\n" + " return (Quaternion)(-q.xyz, q.w);\n" + "}\n" + "__inline\n" + "float4 qtInvRotate(const Quaternion q, float4 vec)\n" + "{\n" + " return qtRotate( qtInvert( q ), vec );\n" + "}\n" + "#define WG_SIZE 64\n" + "typedef struct\n" + "{\n" + " float4 m_pos;\n" + " Quaternion m_quat;\n" + " float4 m_linVel;\n" + " float4 m_angVel;\n" + " u32 m_shapeIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "} Body;\n" + "typedef struct\n" + "{\n" + " Matrix3x3 m_invInertia;\n" + " Matrix3x3 m_initInvInertia;\n" + "} Shape;\n" + "typedef struct\n" + "{\n" + " float4 m_linear;\n" + " float4 m_worldPos[4];\n" + " float4 m_center; \n" + " float m_jacCoeffInv[4];\n" + " float m_b[4];\n" + " float m_appliedRambdaDt[4];\n" + " float m_fJacCoeffInv[2]; \n" + " float m_fAppliedRambdaDt[2]; \n" + " u32 m_bodyA;\n" + " u32 m_bodyB;\n" + " int m_batchIdx;\n" + " u32 m_paddings[1];\n" + "} Constraint4;\n" + "typedef struct\n" + "{\n" + " int m_nConstraints;\n" + " int m_start;\n" + " int m_batchIdx;\n" + " int m_nSplit;\n" + "// int m_paddings[1];\n" + "} ConstBuffer;\n" + "typedef struct\n" + "{\n" + " int m_solveFriction;\n" + " int m_maxBatch; // long batch really kills the performance\n" + " int m_batchIdx;\n" + " int m_nSplit;\n" + "// int m_paddings[1];\n" + "} ConstBufferBatchSolve;\n" + " \n" + "typedef struct \n" + "{\n" + " int m_valInt0;\n" + " int m_valInt1;\n" + " int m_valInt2;\n" + " int m_valInt3;\n" + " float m_val0;\n" + " float m_val1;\n" + " float m_val2;\n" + " float m_val3;\n" + "} SolverDebugInfo;\n" + "// others\n" + "__kernel\n" + "__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void ReorderContactKernel(__global struct b3Contact4Data* in, __global struct b3Contact4Data* out, __global int2* sortData, int4 cb )\n" + "{\n" + " int nContacts = cb.x;\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " if( gIdx < nContacts )\n" + " {\n" + " int srcIdx = sortData[gIdx].y;\n" + " out[gIdx] = in[srcIdx];\n" + " }\n" + "}\n" + "__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void SetDeterminismSortDataChildShapeB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataOut, int nContacts)\n" + "{\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " if( gIdx < nContacts )\n" + " {\n" + " int2 sd;\n" + " sd.x = contactsIn[gIdx].m_childIndexB;\n" + " sd.y = gIdx;\n" + " sortDataOut[gIdx] = sd;\n" + " }\n" + "}\n" + "__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void SetDeterminismSortDataChildShapeA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" + "{\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " if( gIdx < nContacts )\n" + " {\n" + " int2 sdIn;\n" + " sdIn = sortDataInOut[gIdx];\n" + " int2 sdOut;\n" + " sdOut.x = contactsIn[sdIn.y].m_childIndexA;\n" + " sdOut.y = sdIn.y;\n" + " sortDataInOut[gIdx] = sdOut;\n" + " }\n" + "}\n" + "__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void SetDeterminismSortDataBodyA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" + "{\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " if( gIdx < nContacts )\n" + " {\n" + " int2 sdIn;\n" + " sdIn = sortDataInOut[gIdx];\n" + " int2 sdOut;\n" + " sdOut.x = contactsIn[sdIn.y].m_bodyAPtrAndSignBit;\n" + " sdOut.y = sdIn.y;\n" + " sortDataInOut[gIdx] = sdOut;\n" + " }\n" + "}\n" + "__kernel\n" + "__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void SetDeterminismSortDataBodyB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts)\n" + "{\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " if( gIdx < nContacts )\n" + " {\n" + " int2 sdIn;\n" + " sdIn = sortDataInOut[gIdx];\n" + " int2 sdOut;\n" + " sdOut.x = contactsIn[sdIn.y].m_bodyBPtrAndSignBit;\n" + " sdOut.y = sdIn.y;\n" + " sortDataInOut[gIdx] = sdOut;\n" + " }\n" + "}\n" + "typedef struct\n" + "{\n" + " int m_nContacts;\n" + " int m_staticIdx;\n" + " float m_scale;\n" + " int m_nSplit;\n" + "} ConstBufferSSD;\n" + "__constant const int gridTable4x4[] = \n" + "{\n" + " 0,1,17,16,\n" + " 1,2,18,19,\n" + " 17,18,32,3,\n" + " 16,19,3,34\n" + "};\n" + "__constant const int gridTable8x8[] = \n" + "{\n" + " 0, 2, 3, 16, 17, 18, 19, 1,\n" + " 66, 64, 80, 67, 82, 81, 65, 83,\n" + " 131,144,128,130,147,129,145,146,\n" + " 208,195,194,192,193,211,210,209,\n" + " 21, 22, 23, 5, 4, 6, 7, 20,\n" + " 86, 85, 69, 87, 70, 68, 84, 71,\n" + " 151,133,149,150,135,148,132,134,\n" + " 197,27,214,213,212,199,198,196\n" + " \n" + "};\n" + "#define USE_SPATIAL_BATCHING 1\n" + "#define USE_4x4_GRID 1\n" + "__kernel\n" + "__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void SetSortDataKernel(__global struct b3Contact4Data* gContact, __global Body* gBodies, __global int2* gSortDataOut, \n" + "int nContacts,float scale,int4 nSplit,int staticIdx)\n" + "{\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " \n" + " if( gIdx < nContacts )\n" + " {\n" + " int aPtrAndSignBit = gContact[gIdx].m_bodyAPtrAndSignBit;\n" + " int bPtrAndSignBit = gContact[gIdx].m_bodyBPtrAndSignBit;\n" + " int aIdx = abs(aPtrAndSignBit );\n" + " int bIdx = abs(bPtrAndSignBit);\n" + " bool aStatic = (aPtrAndSignBit<0) ||(aPtrAndSignBit==staticIdx);\n" + " bool bStatic = (bPtrAndSignBit<0) ||(bPtrAndSignBit==staticIdx);\n" + "#if USE_SPATIAL_BATCHING \n" + " int idx = (aStatic)? bIdx: aIdx;\n" + " float4 p = gBodies[idx].m_pos;\n" + " int xIdx = (int)((p.x-((p.x<0.f)?1.f:0.f))*scale) & (nSplit.x-1);\n" + " int yIdx = (int)((p.y-((p.y<0.f)?1.f:0.f))*scale) & (nSplit.y-1);\n" + " int zIdx = (int)((p.z-((p.z<0.f)?1.f:0.f))*scale) & (nSplit.z-1);\n" + " int newIndex = (xIdx+yIdx*nSplit.x+zIdx*nSplit.x*nSplit.y);\n" + " \n" + "#else//USE_SPATIAL_BATCHING\n" + " #if USE_4x4_GRID\n" + " int aa = aIdx&3;\n" + " int bb = bIdx&3;\n" + " if (aStatic)\n" + " aa = bb;\n" + " if (bStatic)\n" + " bb = aa;\n" + " int gridIndex = aa + bb*4;\n" + " int newIndex = gridTable4x4[gridIndex];\n" + " #else//USE_4x4_GRID\n" + " int aa = aIdx&7;\n" + " int bb = bIdx&7;\n" + " if (aStatic)\n" + " aa = bb;\n" + " if (bStatic)\n" + " bb = aa;\n" + " int gridIndex = aa + bb*8;\n" + " int newIndex = gridTable8x8[gridIndex];\n" + " #endif//USE_4x4_GRID\n" + "#endif//USE_SPATIAL_BATCHING\n" + " gSortDataOut[gIdx].x = newIndex;\n" + " gSortDataOut[gIdx].y = gIdx;\n" + " }\n" + " else\n" + " {\n" + " gSortDataOut[gIdx].x = 0xffffffff;\n" + " }\n" + "}\n" + "__kernel\n" + "__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void CopyConstraintKernel(__global struct b3Contact4Data* gIn, __global struct b3Contact4Data* gOut, int4 cb )\n" + "{\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " if( gIdx < cb.x )\n" + " {\n" + " gOut[gIdx] = gIn[gIdx];\n" + " }\n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverUtils.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverUtils.h index c0173ad9f4..f4d98d9941 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverUtils.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverUtils.h @@ -1,909 +1,908 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* solverUtilsCL= \ -"/*\n" -"Copyright (c) 2013 Advanced Micro Devices, Inc. \n" -"This software is provided 'as-is', without any express or implied warranty.\n" -"In no event will the authors be held liable for any damages arising from the use of this software.\n" -"Permission is granted to anyone to use this software for any purpose, \n" -"including commercial applications, and to alter it and redistribute it freely, \n" -"subject to the following restrictions:\n" -"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" -"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" -"3. This notice may not be removed or altered from any source distribution.\n" -"*/\n" -"//Originally written by Erwin Coumans\n" -"#ifndef B3_CONTACT4DATA_H\n" -"#define B3_CONTACT4DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#define B3_FLOAT4_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#define B3_PLATFORM_DEFINITIONS_H\n" -"struct MyTest\n" -"{\n" -" int bla;\n" -"};\n" -"#ifdef __cplusplus\n" -"#else\n" -"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" -"#define B3_LARGE_FLOAT 1e18f\n" -"#define B3_INFINITY 1e18f\n" -"#define b3Assert(a)\n" -"#define b3ConstArray(a) __global const a*\n" -"#define b3AtomicInc atomic_inc\n" -"#define b3AtomicAdd atomic_add\n" -"#define b3Fabs fabs\n" -"#define b3Sqrt native_sqrt\n" -"#define b3Sin native_sin\n" -"#define b3Cos native_cos\n" -"#define B3_STATIC\n" -"#endif\n" -"#endif\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Float4;\n" -" #define b3Float4ConstArg const b3Float4\n" -" #define b3MakeFloat4 (float4)\n" -" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return dot(a1, b1);\n" -" }\n" -" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return cross(a1, b1);\n" -" }\n" -" #define b3MinFloat4 min\n" -" #define b3MaxFloat4 max\n" -" #define b3Normalized(a) normalize(a)\n" -"#endif \n" -" \n" -"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" -"{\n" -" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" -" return false;\n" -" return true;\n" -"}\n" -"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" -"{\n" -" float maxDot = -B3_INFINITY;\n" -" int i = 0;\n" -" int ptIndex = -1;\n" -" for( i = 0; i < vecLen; i++ )\n" -" {\n" -" float dot = b3Dot3F4(vecArray[i],vec);\n" -" \n" -" if( dot > maxDot )\n" -" {\n" -" maxDot = dot;\n" -" ptIndex = i;\n" -" }\n" -" }\n" -" b3Assert(ptIndex>=0);\n" -" if (ptIndex<0)\n" -" {\n" -" ptIndex = 0;\n" -" }\n" -" *dotOut = maxDot;\n" -" return ptIndex;\n" -"}\n" -"#endif //B3_FLOAT4_H\n" -"typedef struct b3Contact4Data b3Contact4Data_t;\n" -"struct b3Contact4Data\n" -"{\n" -" b3Float4 m_worldPosB[4];\n" -"// b3Float4 m_localPosA[4];\n" -"// b3Float4 m_localPosB[4];\n" -" b3Float4 m_worldNormalOnB; // w: m_nPoints\n" -" unsigned short m_restituitionCoeffCmp;\n" -" unsigned short m_frictionCoeffCmp;\n" -" int m_batchIdx;\n" -" int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" -" int m_bodyBPtrAndSignBit;\n" -" int m_childIndexA;\n" -" int m_childIndexB;\n" -" int m_unused1;\n" -" int m_unused2;\n" -"};\n" -"inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" -"{\n" -" return (int)contact->m_worldNormalOnB.w;\n" -"};\n" -"inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" -"{\n" -" contact->m_worldNormalOnB.w = (float)numPoints;\n" -"};\n" -"#endif //B3_CONTACT4DATA_H\n" -"#pragma OPENCL EXTENSION cl_amd_printf : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" -"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" -"#ifdef cl_ext_atomic_counters_32\n" -"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" -"#else\n" -"#define counter32_t volatile global int*\n" -"#endif\n" -"typedef unsigned int u32;\n" -"typedef unsigned short u16;\n" -"typedef unsigned char u8;\n" -"#define GET_GROUP_IDX get_group_id(0)\n" -"#define GET_LOCAL_IDX get_local_id(0)\n" -"#define GET_GLOBAL_IDX get_global_id(0)\n" -"#define GET_GROUP_SIZE get_local_size(0)\n" -"#define GET_NUM_GROUPS get_num_groups(0)\n" -"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" -"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" -"#define AtomInc(x) atom_inc(&(x))\n" -"#define AtomInc1(x, out) out = atom_inc(&(x))\n" -"#define AppendInc(x, out) out = atomic_inc(x)\n" -"#define AtomAdd(x, value) atom_add(&(x), value)\n" -"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" -"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" -"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" -"#define make_float4 (float4)\n" -"#define make_float2 (float2)\n" -"#define make_uint4 (uint4)\n" -"#define make_int4 (int4)\n" -"#define make_uint2 (uint2)\n" -"#define make_int2 (int2)\n" -"#define max2 max\n" -"#define min2 min\n" -"///////////////////////////////////////\n" -"// Vector\n" -"///////////////////////////////////////\n" -"__inline\n" -"float fastDiv(float numerator, float denominator)\n" -"{\n" -" return native_divide(numerator, denominator); \n" -"// return numerator/denominator; \n" -"}\n" -"__inline\n" -"float4 fastDiv4(float4 numerator, float4 denominator)\n" -"{\n" -" return native_divide(numerator, denominator); \n" -"}\n" -"__inline\n" -"float fastSqrtf(float f2)\n" -"{\n" -" return native_sqrt(f2);\n" -"// return sqrt(f2);\n" -"}\n" -"__inline\n" -"float fastRSqrt(float f2)\n" -"{\n" -" return native_rsqrt(f2);\n" -"}\n" -"__inline\n" -"float fastLength4(float4 v)\n" -"{\n" -" return fast_length(v);\n" -"}\n" -"__inline\n" -"float4 fastNormalize4(float4 v)\n" -"{\n" -" return fast_normalize(v);\n" -"}\n" -"__inline\n" -"float sqrtf(float a)\n" -"{\n" -"// return sqrt(a);\n" -" return native_sqrt(a);\n" -"}\n" -"__inline\n" -"float4 cross3(float4 a1, float4 b1)\n" -"{\n" -" float4 a=make_float4(a1.xyz,0.f);\n" -" float4 b=make_float4(b1.xyz,0.f);\n" -" //float4 a=a1;\n" -" //float4 b=b1;\n" -" return cross(a,b);\n" -"}\n" -"__inline\n" -"float dot3F4(float4 a, float4 b)\n" -"{\n" -" float4 a1 = make_float4(a.xyz,0.f);\n" -" float4 b1 = make_float4(b.xyz,0.f);\n" -" return dot(a1, b1);\n" -"}\n" -"__inline\n" -"float length3(const float4 a)\n" -"{\n" -" return sqrtf(dot3F4(a,a));\n" -"}\n" -"__inline\n" -"float dot4(const float4 a, const float4 b)\n" -"{\n" -" return dot( a, b );\n" -"}\n" -"// for height\n" -"__inline\n" -"float dot3w1(const float4 point, const float4 eqn)\n" -"{\n" -" return dot3F4(point,eqn) + eqn.w;\n" -"}\n" -"__inline\n" -"float4 normalize3(const float4 a)\n" -"{\n" -" float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" -" return fastNormalize4( n );\n" -"// float length = sqrtf(dot3F4(a, a));\n" -"// return 1.f/length * a;\n" -"}\n" -"__inline\n" -"float4 normalize4(const float4 a)\n" -"{\n" -" float length = sqrtf(dot4(a, a));\n" -" return 1.f/length * a;\n" -"}\n" -"__inline\n" -"float4 createEquation(const float4 a, const float4 b, const float4 c)\n" -"{\n" -" float4 eqn;\n" -" float4 ab = b-a;\n" -" float4 ac = c-a;\n" -" eqn = normalize3( cross3(ab, ac) );\n" -" eqn.w = -dot3F4(eqn,a);\n" -" return eqn;\n" -"}\n" -"///////////////////////////////////////\n" -"// Matrix3x3\n" -"///////////////////////////////////////\n" -"typedef struct\n" -"{\n" -" float4 m_row[3];\n" -"}Matrix3x3;\n" -"__inline\n" -"Matrix3x3 mtZero();\n" -"__inline\n" -"Matrix3x3 mtIdentity();\n" -"__inline\n" -"Matrix3x3 mtTranspose(Matrix3x3 m);\n" -"__inline\n" -"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b);\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b);\n" -"__inline\n" -"Matrix3x3 mtZero()\n" -"{\n" -" Matrix3x3 m;\n" -" m.m_row[0] = (float4)(0.f);\n" -" m.m_row[1] = (float4)(0.f);\n" -" m.m_row[2] = (float4)(0.f);\n" -" return m;\n" -"}\n" -"__inline\n" -"Matrix3x3 mtIdentity()\n" -"{\n" -" Matrix3x3 m;\n" -" m.m_row[0] = (float4)(1,0,0,0);\n" -" m.m_row[1] = (float4)(0,1,0,0);\n" -" m.m_row[2] = (float4)(0,0,1,0);\n" -" return m;\n" -"}\n" -"__inline\n" -"Matrix3x3 mtTranspose(Matrix3x3 m)\n" -"{\n" -" Matrix3x3 out;\n" -" out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" -" out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" -" out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" -" return out;\n" -"}\n" -"__inline\n" -"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n" -"{\n" -" Matrix3x3 transB;\n" -" transB = mtTranspose( b );\n" -" Matrix3x3 ans;\n" -" // why this doesn't run when 0ing in the for{}\n" -" a.m_row[0].w = 0.f;\n" -" a.m_row[1].w = 0.f;\n" -" a.m_row[2].w = 0.f;\n" -" for(int i=0; i<3; i++)\n" -" {\n" -"// a.m_row[i].w = 0.f;\n" -" ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n" -" ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n" -" ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n" -" ans.m_row[i].w = 0.f;\n" -" }\n" -" return ans;\n" -"}\n" -"__inline\n" -"float4 mtMul1(Matrix3x3 a, float4 b)\n" -"{\n" -" float4 ans;\n" -" ans.x = dot3F4( a.m_row[0], b );\n" -" ans.y = dot3F4( a.m_row[1], b );\n" -" ans.z = dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"float4 mtMul3(float4 a, Matrix3x3 b)\n" -"{\n" -" float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" float4 ans;\n" -" ans.x = dot3F4( a, colx );\n" -" ans.y = dot3F4( a, coly );\n" -" ans.z = dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"///////////////////////////////////////\n" -"// Quaternion\n" -"///////////////////////////////////////\n" -"typedef float4 Quaternion;\n" -"__inline\n" -"Quaternion qtMul(Quaternion a, Quaternion b);\n" -"__inline\n" -"Quaternion qtNormalize(Quaternion in);\n" -"__inline\n" -"float4 qtRotate(Quaternion q, float4 vec);\n" -"__inline\n" -"Quaternion qtInvert(Quaternion q);\n" -"__inline\n" -"Quaternion qtMul(Quaternion a, Quaternion b)\n" -"{\n" -" Quaternion ans;\n" -" ans = cross3( a, b );\n" -" ans += a.w*b+b.w*a;\n" -"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" -" ans.w = a.w*b.w - dot3F4(a, b);\n" -" return ans;\n" -"}\n" -"__inline\n" -"Quaternion qtNormalize(Quaternion in)\n" -"{\n" -" return fastNormalize4(in);\n" -"// in /= length( in );\n" -"// return in;\n" -"}\n" -"__inline\n" -"float4 qtRotate(Quaternion q, float4 vec)\n" -"{\n" -" Quaternion qInv = qtInvert( q );\n" -" float4 vcpy = vec;\n" -" vcpy.w = 0.f;\n" -" float4 out = qtMul(qtMul(q,vcpy),qInv);\n" -" return out;\n" -"}\n" -"__inline\n" -"Quaternion qtInvert(Quaternion q)\n" -"{\n" -" return (Quaternion)(-q.xyz, q.w);\n" -"}\n" -"__inline\n" -"float4 qtInvRotate(const Quaternion q, float4 vec)\n" -"{\n" -" return qtRotate( qtInvert( q ), vec );\n" -"}\n" -"#define WG_SIZE 64\n" -"typedef struct\n" -"{\n" -" float4 m_pos;\n" -" Quaternion m_quat;\n" -" float4 m_linVel;\n" -" float4 m_angVel;\n" -" u32 m_shapeIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"} Body;\n" -"typedef struct\n" -"{\n" -" Matrix3x3 m_invInertia;\n" -" Matrix3x3 m_initInvInertia;\n" -"} Shape;\n" -"typedef struct\n" -"{\n" -" float4 m_linear;\n" -" float4 m_worldPos[4];\n" -" float4 m_center; \n" -" float m_jacCoeffInv[4];\n" -" float m_b[4];\n" -" float m_appliedRambdaDt[4];\n" -" float m_fJacCoeffInv[2]; \n" -" float m_fAppliedRambdaDt[2]; \n" -" u32 m_bodyA;\n" -" u32 m_bodyB;\n" -" int m_batchIdx;\n" -" u32 m_paddings;\n" -"} Constraint4;\n" -"__kernel void CountBodiesKernel(__global struct b3Contact4Data* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex)\n" -"{\n" -" int i = GET_GLOBAL_IDX;\n" -" \n" -" if( i < numContactManifolds)\n" -" {\n" -" int pa = manifoldPtr[i].m_bodyAPtrAndSignBit;\n" -" bool isFixedA = (pa <0) || (pa == fixedBodyIndex);\n" -" int bodyIndexA = abs(pa);\n" -" if (!isFixedA)\n" -" {\n" -" AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x);\n" -" }\n" -" barrier(CLK_GLOBAL_MEM_FENCE);\n" -" int pb = manifoldPtr[i].m_bodyBPtrAndSignBit;\n" -" bool isFixedB = (pb <0) || (pb == fixedBodyIndex);\n" -" int bodyIndexB = abs(pb);\n" -" if (!isFixedB)\n" -" {\n" -" AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y);\n" -" } \n" -" }\n" -"}\n" -"__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies)\n" -"{\n" -" int i = GET_GLOBAL_IDX;\n" -" \n" -" if( i < numSplitBodies)\n" -" {\n" -" linearVelocities[i] = make_float4(0);\n" -" angularVelocities[i] = make_float4(0);\n" -" }\n" -"}\n" -"__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,\n" -"__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)\n" -"{\n" -" int i = GET_GLOBAL_IDX;\n" -" if (i<numBodies)\n" -" {\n" -" if (gBodies[i].m_invMass)\n" -" {\n" -" int bodyOffset = offsetSplitBodies[i];\n" -" int count = bodyCount[i];\n" -" float factor = 1.f/((float)count);\n" -" float4 averageLinVel = make_float4(0.f);\n" -" float4 averageAngVel = make_float4(0.f);\n" -" \n" -" for (int j=0;j<count;j++)\n" -" {\n" -" averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;\n" -" averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;\n" -" }\n" -" \n" -" for (int j=0;j<count;j++)\n" -" {\n" -" deltaLinearVelocities[bodyOffset+j] = averageLinVel;\n" -" deltaAngularVelocities[bodyOffset+j] = averageAngVel;\n" -" }\n" -" \n" -" }//bodies[i].m_invMass\n" -" }//i<numBodies\n" -"}\n" -"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n" -"{\n" -" *linear = make_float4(n.xyz,0.f);\n" -" *angular0 = cross3(r0, n);\n" -" *angular1 = -cross3(r1, n);\n" -"}\n" -"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n" -"{\n" -" return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n" -"}\n" -"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" -" float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1, float countA, float countB)\n" -"{\n" -" // linear0,1 are normlized\n" -" float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n" -" float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" -" float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n" -" float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" -" return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB);\n" -"}\n" -"void btPlaneSpace1 (float4 n, float4* p, float4* q);\n" -" void btPlaneSpace1 (float4 n, float4* p, float4* q)\n" -"{\n" -" if (fabs(n.z) > 0.70710678f) {\n" -" // choose p in y-z plane\n" -" float a = n.y*n.y + n.z*n.z;\n" -" float k = 1.f/sqrt(a);\n" -" p[0].x = 0;\n" -" p[0].y = -n.z*k;\n" -" p[0].z = n.y*k;\n" -" // set q = n x p\n" -" q[0].x = a*k;\n" -" q[0].y = -n.x*p[0].z;\n" -" q[0].z = n.x*p[0].y;\n" -" }\n" -" else {\n" -" // choose p in x-y plane\n" -" float a = n.x*n.x + n.y*n.y;\n" -" float k = 1.f/sqrt(a);\n" -" p[0].x = -n.y*k;\n" -" p[0].y = n.x*k;\n" -" p[0].z = 0;\n" -" // set q = n x p\n" -" q[0].x = -n.z*p[0].y;\n" -" q[0].y = n.z*p[0].x;\n" -" q[0].z = a*k;\n" -" }\n" -"}\n" -"void solveContact(__global Constraint4* cs,\n" -" float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n" -" float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB,\n" -" float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB)\n" -"{\n" -" float minRambdaDt = 0;\n" -" float maxRambdaDt = FLT_MAX;\n" -" for(int ic=0; ic<4; ic++)\n" -" {\n" -" if( cs->m_jacCoeffInv[ic] == 0.f ) continue;\n" -" float4 angular0, angular1, linear;\n" -" float4 r0 = cs->m_worldPos[ic] - posA;\n" -" float4 r1 = cs->m_worldPos[ic] - posB;\n" -" setLinearAndAngular( cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n" -" \n" -" float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n" -" *linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];\n" -" rambdaDt *= cs->m_jacCoeffInv[ic];\n" -" \n" -" {\n" -" float prevSum = cs->m_appliedRambdaDt[ic];\n" -" float updated = prevSum;\n" -" updated += rambdaDt;\n" -" updated = max2( updated, minRambdaDt );\n" -" updated = min2( updated, maxRambdaDt );\n" -" rambdaDt = updated - prevSum;\n" -" cs->m_appliedRambdaDt[ic] = updated;\n" -" }\n" -" \n" -" float4 linImp0 = invMassA*linear*rambdaDt;\n" -" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" -" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" -" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" -" \n" -" if (invMassA)\n" -" {\n" -" *dLinVelA += linImp0;\n" -" *dAngVelA += angImp0;\n" -" }\n" -" if (invMassB)\n" -" {\n" -" *dLinVelB += linImp1;\n" -" *dAngVelB += angImp1;\n" -" }\n" -" }\n" -"}\n" -"// solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n" -"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, \n" -"__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n" -"__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n" -"{\n" -" //float frictionCoeff = ldsCs[0].m_linear.w;\n" -" int aIdx = ldsCs[0].m_bodyA;\n" -" int bIdx = ldsCs[0].m_bodyB;\n" -" float4 posA = gBodies[aIdx].m_pos;\n" -" float4 linVelA = gBodies[aIdx].m_linVel;\n" -" float4 angVelA = gBodies[aIdx].m_angVel;\n" -" float invMassA = gBodies[aIdx].m_invMass;\n" -" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" -" float4 posB = gBodies[bIdx].m_pos;\n" -" float4 linVelB = gBodies[bIdx].m_linVel;\n" -" float4 angVelB = gBodies[bIdx].m_angVel;\n" -" float invMassB = gBodies[bIdx].m_invMass;\n" -" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" -" \n" -" float4 dLinVelA = make_float4(0,0,0,0);\n" -" float4 dAngVelA = make_float4(0,0,0,0);\n" -" float4 dLinVelB = make_float4(0,0,0,0);\n" -" float4 dAngVelB = make_float4(0,0,0,0);\n" -" \n" -" int bodyOffsetA = offsetSplitBodies[aIdx];\n" -" int constraintOffsetA = contactConstraintOffsets[0].x;\n" -" int splitIndexA = bodyOffsetA+constraintOffsetA;\n" -" \n" -" if (invMassA)\n" -" {\n" -" dLinVelA = deltaLinearVelocities[splitIndexA];\n" -" dAngVelA = deltaAngularVelocities[splitIndexA];\n" -" }\n" -" int bodyOffsetB = offsetSplitBodies[bIdx];\n" -" int constraintOffsetB = contactConstraintOffsets[0].y;\n" -" int splitIndexB= bodyOffsetB+constraintOffsetB;\n" -" if (invMassB)\n" -" {\n" -" dLinVelB = deltaLinearVelocities[splitIndexB];\n" -" dAngVelB = deltaAngularVelocities[splitIndexB];\n" -" }\n" -" solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" -" posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB);\n" -" if (invMassA)\n" -" {\n" -" deltaLinearVelocities[splitIndexA] = dLinVelA;\n" -" deltaAngularVelocities[splitIndexA] = dAngVelA;\n" -" } \n" -" if (invMassB)\n" -" {\n" -" deltaLinearVelocities[splitIndexB] = dLinVelB;\n" -" deltaAngularVelocities[splitIndexB] = dAngVelB;\n" -" }\n" -"}\n" -"__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n" -"__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n" -"float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n" -")\n" -"{\n" -" int i = GET_GLOBAL_IDX;\n" -" if (i<numManifolds)\n" -" {\n" -" solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n" -" }\n" -"}\n" -"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs,\n" -" __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n" -" __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n" -"{\n" -" float frictionCoeff = 0.7f;//ldsCs[0].m_linear.w;\n" -" int aIdx = ldsCs[0].m_bodyA;\n" -" int bIdx = ldsCs[0].m_bodyB;\n" -" float4 posA = gBodies[aIdx].m_pos;\n" -" float4 linVelA = gBodies[aIdx].m_linVel;\n" -" float4 angVelA = gBodies[aIdx].m_angVel;\n" -" float invMassA = gBodies[aIdx].m_invMass;\n" -" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" -" float4 posB = gBodies[bIdx].m_pos;\n" -" float4 linVelB = gBodies[bIdx].m_linVel;\n" -" float4 angVelB = gBodies[bIdx].m_angVel;\n" -" float invMassB = gBodies[bIdx].m_invMass;\n" -" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" -" \n" -" float4 dLinVelA = make_float4(0,0,0,0);\n" -" float4 dAngVelA = make_float4(0,0,0,0);\n" -" float4 dLinVelB = make_float4(0,0,0,0);\n" -" float4 dAngVelB = make_float4(0,0,0,0);\n" -" \n" -" int bodyOffsetA = offsetSplitBodies[aIdx];\n" -" int constraintOffsetA = contactConstraintOffsets[0].x;\n" -" int splitIndexA = bodyOffsetA+constraintOffsetA;\n" -" \n" -" if (invMassA)\n" -" {\n" -" dLinVelA = deltaLinearVelocities[splitIndexA];\n" -" dAngVelA = deltaAngularVelocities[splitIndexA];\n" -" }\n" -" int bodyOffsetB = offsetSplitBodies[bIdx];\n" -" int constraintOffsetB = contactConstraintOffsets[0].y;\n" -" int splitIndexB= bodyOffsetB+constraintOffsetB;\n" -" if (invMassB)\n" -" {\n" -" dLinVelB = deltaLinearVelocities[splitIndexB];\n" -" dAngVelB = deltaAngularVelocities[splitIndexB];\n" -" }\n" -" {\n" -" float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};\n" -" float minRambdaDt[4] = {0.f,0.f,0.f,0.f};\n" -" float sum = 0;\n" -" for(int j=0; j<4; j++)\n" -" {\n" -" sum +=ldsCs[0].m_appliedRambdaDt[j];\n" -" }\n" -" frictionCoeff = 0.7f;\n" -" for(int j=0; j<4; j++)\n" -" {\n" -" maxRambdaDt[j] = frictionCoeff*sum;\n" -" minRambdaDt[j] = -maxRambdaDt[j];\n" -" }\n" -" \n" -"// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" -"// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );\n" -" \n" -" \n" -" {\n" -" \n" -" __global Constraint4* cs = ldsCs;\n" -" \n" -" if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n" -" const float4 center = cs->m_center;\n" -" \n" -" float4 n = -cs->m_linear;\n" -" \n" -" float4 tangent[2];\n" -" btPlaneSpace1(n,&tangent[0],&tangent[1]);\n" -" float4 angular0, angular1, linear;\n" -" float4 r0 = center - posA;\n" -" float4 r1 = center - posB;\n" -" for(int i=0; i<2; i++)\n" -" {\n" -" setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n" -" float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n" -" linVelA+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB );\n" -" rambdaDt *= cs->m_fJacCoeffInv[i];\n" -" \n" -" {\n" -" float prevSum = cs->m_fAppliedRambdaDt[i];\n" -" float updated = prevSum;\n" -" updated += rambdaDt;\n" -" updated = max2( updated, minRambdaDt[i] );\n" -" updated = min2( updated, maxRambdaDt[i] );\n" -" rambdaDt = updated - prevSum;\n" -" cs->m_fAppliedRambdaDt[i] = updated;\n" -" }\n" -" \n" -" float4 linImp0 = invMassA*linear*rambdaDt;\n" -" float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" -" float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" -" float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" -" \n" -" dLinVelA += linImp0;\n" -" dAngVelA += angImp0;\n" -" dLinVelB += linImp1;\n" -" dAngVelB += angImp1;\n" -" }\n" -" { // angular damping for point constraint\n" -" float4 ab = normalize3( posB - posA );\n" -" float4 ac = normalize3( center - posA );\n" -" if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))\n" -" {\n" -" float angNA = dot3F4( n, angVelA );\n" -" float angNB = dot3F4( n, angVelB );\n" -" \n" -" dAngVelA -= (angNA*0.1f)*n;\n" -" dAngVelB -= (angNB*0.1f)*n;\n" -" }\n" -" }\n" -" }\n" -" \n" -" \n" -" }\n" -" if (invMassA)\n" -" {\n" -" deltaLinearVelocities[splitIndexA] = dLinVelA;\n" -" deltaAngularVelocities[splitIndexA] = dAngVelA;\n" -" } \n" -" if (invMassB)\n" -" {\n" -" deltaLinearVelocities[splitIndexB] = dLinVelB;\n" -" deltaAngularVelocities[splitIndexB] = dAngVelB;\n" -" }\n" -" \n" -"}\n" -"__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n" -" __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n" -" __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n" -" float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n" -")\n" -"{\n" -" int i = GET_GLOBAL_IDX;\n" -" if (i<numManifolds)\n" -" {\n" -" solveFrictionConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n" -" }\n" -"}\n" -"__kernel void UpdateBodyVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,\n" -" __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)\n" -"{\n" -" int i = GET_GLOBAL_IDX;\n" -" if (i<numBodies)\n" -" {\n" -" if (gBodies[i].m_invMass)\n" -" {\n" -" int bodyOffset = offsetSplitBodies[i];\n" -" int count = bodyCount[i];\n" -" if (count)\n" -" {\n" -" gBodies[i].m_linVel += deltaLinearVelocities[bodyOffset];\n" -" gBodies[i].m_angVel += deltaAngularVelocities[bodyOffset];\n" -" }\n" -" }\n" -" }\n" -"}\n" -"void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVelA, float invMassA, const Matrix3x3 invInertiaA,\n" -" const float4 posB, const float4 linVelB, const float4 angVelB, float invMassB, const Matrix3x3 invInertiaB, \n" -" __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,float countA, float countB,\n" -" Constraint4* dstC )\n" -"{\n" -" dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);\n" -" dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n" -" float dtInv = 1.f/dt;\n" -" for(int ic=0; ic<4; ic++)\n" -" {\n" -" dstC->m_appliedRambdaDt[ic] = 0.f;\n" -" }\n" -" dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n" -" dstC->m_linear = src->m_worldNormalOnB;\n" -" dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n" -" for(int ic=0; ic<4; ic++)\n" -" {\n" -" float4 r0 = src->m_worldPosB[ic] - posA;\n" -" float4 r1 = src->m_worldPosB[ic] - posB;\n" -" if( ic >= src->m_worldNormalOnB.w )//npoints\n" -" {\n" -" dstC->m_jacCoeffInv[ic] = 0.f;\n" -" continue;\n" -" }\n" -" float relVelN;\n" -" {\n" -" float4 linear, angular0, angular1;\n" -" setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);\n" -" dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n" -" invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB);\n" -" relVelN = calcRelVel(linear, -linear, angular0, angular1,\n" -" linVelA, angVelA, linVelB, angVelB);\n" -" float e = 0.f;//src->getRestituitionCoeff();\n" -" if( relVelN*relVelN < 0.004f ) e = 0.f;\n" -" dstC->m_b[ic] = e*relVelN;\n" -" //float penetration = src->m_worldPosB[ic].w;\n" -" dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n" -" dstC->m_appliedRambdaDt[ic] = 0.f;\n" -" }\n" -" }\n" -" if( src->m_worldNormalOnB.w > 0 )//npoints\n" -" { // prepare friction\n" -" float4 center = make_float4(0.f);\n" -" for(int i=0; i<src->m_worldNormalOnB.w; i++) \n" -" center += src->m_worldPosB[i];\n" -" center /= (float)src->m_worldNormalOnB.w;\n" -" float4 tangent[2];\n" -" btPlaneSpace1(-src->m_worldNormalOnB,&tangent[0],&tangent[1]);\n" -" \n" -" float4 r[2];\n" -" r[0] = center - posA;\n" -" r[1] = center - posB;\n" -" for(int i=0; i<2; i++)\n" -" {\n" -" float4 linear, angular0, angular1;\n" -" setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n" -" dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n" -" invMassA, &invInertiaA, invMassB, &invInertiaB ,countA, countB);\n" -" dstC->m_fAppliedRambdaDt[i] = 0.f;\n" -" }\n" -" dstC->m_center = center;\n" -" }\n" -" for(int i=0; i<4; i++)\n" -" {\n" -" if( i<src->m_worldNormalOnB.w )\n" -" {\n" -" dstC->m_worldPos[i] = src->m_worldPosB[i];\n" -" }\n" -" else\n" -" {\n" -" dstC->m_worldPos[i] = make_float4(0.f);\n" -" }\n" -" }\n" -"}\n" -"__kernel\n" -"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" -"void ContactToConstraintSplitKernel(__global const struct b3Contact4Data* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, \n" -"__global const unsigned int* bodyCount,\n" -"int nContacts,\n" -"float dt,\n" -"float positionDrift,\n" -"float positionConstraintCoeff\n" -")\n" -"{\n" -" int gIdx = GET_GLOBAL_IDX;\n" -" \n" -" if( gIdx < nContacts )\n" -" {\n" -" int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n" -" int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n" -" float4 posA = gBodies[aIdx].m_pos;\n" -" float4 linVelA = gBodies[aIdx].m_linVel;\n" -" float4 angVelA = gBodies[aIdx].m_angVel;\n" -" float invMassA = gBodies[aIdx].m_invMass;\n" -" Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" -" float4 posB = gBodies[bIdx].m_pos;\n" -" float4 linVelB = gBodies[bIdx].m_linVel;\n" -" float4 angVelB = gBodies[bIdx].m_angVel;\n" -" float invMassB = gBodies[bIdx].m_invMass;\n" -" Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" -" Constraint4 cs;\n" -" float countA = invMassA != 0.f ? (float)bodyCount[aIdx] : 1;\n" -" float countB = invMassB != 0.f ? (float)bodyCount[bIdx] : 1;\n" -" setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n" -" &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB,\n" -" &cs );\n" -" \n" -" cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n" -" gConstraintOut[gIdx] = cs;\n" -" }\n" -"}\n" -; +static const char* solverUtilsCL = + "/*\n" + "Copyright (c) 2013 Advanced Micro Devices, Inc. \n" + "This software is provided 'as-is', without any express or implied warranty.\n" + "In no event will the authors be held liable for any damages arising from the use of this software.\n" + "Permission is granted to anyone to use this software for any purpose, \n" + "including commercial applications, and to alter it and redistribute it freely, \n" + "subject to the following restrictions:\n" + "1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n" + "2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n" + "3. This notice may not be removed or altered from any source distribution.\n" + "*/\n" + "//Originally written by Erwin Coumans\n" + "#ifndef B3_CONTACT4DATA_H\n" + "#define B3_CONTACT4DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#define B3_FLOAT4_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#define B3_PLATFORM_DEFINITIONS_H\n" + "struct MyTest\n" + "{\n" + " int bla;\n" + "};\n" + "#ifdef __cplusplus\n" + "#else\n" + "//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" + "#define B3_LARGE_FLOAT 1e18f\n" + "#define B3_INFINITY 1e18f\n" + "#define b3Assert(a)\n" + "#define b3ConstArray(a) __global const a*\n" + "#define b3AtomicInc atomic_inc\n" + "#define b3AtomicAdd atomic_add\n" + "#define b3Fabs fabs\n" + "#define b3Sqrt native_sqrt\n" + "#define b3Sin native_sin\n" + "#define b3Cos native_cos\n" + "#define B3_STATIC\n" + "#endif\n" + "#endif\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Float4;\n" + " #define b3Float4ConstArg const b3Float4\n" + " #define b3MakeFloat4 (float4)\n" + " float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return dot(a1, b1);\n" + " }\n" + " b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return cross(a1, b1);\n" + " }\n" + " #define b3MinFloat4 min\n" + " #define b3MaxFloat4 max\n" + " #define b3Normalized(a) normalize(a)\n" + "#endif \n" + " \n" + "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" + "{\n" + " if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" + " return false;\n" + " return true;\n" + "}\n" + "inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" + "{\n" + " float maxDot = -B3_INFINITY;\n" + " int i = 0;\n" + " int ptIndex = -1;\n" + " for( i = 0; i < vecLen; i++ )\n" + " {\n" + " float dot = b3Dot3F4(vecArray[i],vec);\n" + " \n" + " if( dot > maxDot )\n" + " {\n" + " maxDot = dot;\n" + " ptIndex = i;\n" + " }\n" + " }\n" + " b3Assert(ptIndex>=0);\n" + " if (ptIndex<0)\n" + " {\n" + " ptIndex = 0;\n" + " }\n" + " *dotOut = maxDot;\n" + " return ptIndex;\n" + "}\n" + "#endif //B3_FLOAT4_H\n" + "typedef struct b3Contact4Data b3Contact4Data_t;\n" + "struct b3Contact4Data\n" + "{\n" + " b3Float4 m_worldPosB[4];\n" + "// b3Float4 m_localPosA[4];\n" + "// b3Float4 m_localPosB[4];\n" + " b3Float4 m_worldNormalOnB; // w: m_nPoints\n" + " unsigned short m_restituitionCoeffCmp;\n" + " unsigned short m_frictionCoeffCmp;\n" + " int m_batchIdx;\n" + " int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr\n" + " int m_bodyBPtrAndSignBit;\n" + " int m_childIndexA;\n" + " int m_childIndexB;\n" + " int m_unused1;\n" + " int m_unused2;\n" + "};\n" + "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n" + "{\n" + " return (int)contact->m_worldNormalOnB.w;\n" + "};\n" + "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n" + "{\n" + " contact->m_worldNormalOnB.w = (float)numPoints;\n" + "};\n" + "#endif //B3_CONTACT4DATA_H\n" + "#pragma OPENCL EXTENSION cl_amd_printf : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n" + "#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n" + "#ifdef cl_ext_atomic_counters_32\n" + "#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n" + "#else\n" + "#define counter32_t volatile global int*\n" + "#endif\n" + "typedef unsigned int u32;\n" + "typedef unsigned short u16;\n" + "typedef unsigned char u8;\n" + "#define GET_GROUP_IDX get_group_id(0)\n" + "#define GET_LOCAL_IDX get_local_id(0)\n" + "#define GET_GLOBAL_IDX get_global_id(0)\n" + "#define GET_GROUP_SIZE get_local_size(0)\n" + "#define GET_NUM_GROUPS get_num_groups(0)\n" + "#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n" + "#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n" + "#define AtomInc(x) atom_inc(&(x))\n" + "#define AtomInc1(x, out) out = atom_inc(&(x))\n" + "#define AppendInc(x, out) out = atomic_inc(x)\n" + "#define AtomAdd(x, value) atom_add(&(x), value)\n" + "#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n" + "#define AtomXhg(x, value) atom_xchg ( &(x), value )\n" + "#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n" + "#define make_float4 (float4)\n" + "#define make_float2 (float2)\n" + "#define make_uint4 (uint4)\n" + "#define make_int4 (int4)\n" + "#define make_uint2 (uint2)\n" + "#define make_int2 (int2)\n" + "#define max2 max\n" + "#define min2 min\n" + "///////////////////////////////////////\n" + "// Vector\n" + "///////////////////////////////////////\n" + "__inline\n" + "float fastDiv(float numerator, float denominator)\n" + "{\n" + " return native_divide(numerator, denominator); \n" + "// return numerator/denominator; \n" + "}\n" + "__inline\n" + "float4 fastDiv4(float4 numerator, float4 denominator)\n" + "{\n" + " return native_divide(numerator, denominator); \n" + "}\n" + "__inline\n" + "float fastSqrtf(float f2)\n" + "{\n" + " return native_sqrt(f2);\n" + "// return sqrt(f2);\n" + "}\n" + "__inline\n" + "float fastRSqrt(float f2)\n" + "{\n" + " return native_rsqrt(f2);\n" + "}\n" + "__inline\n" + "float fastLength4(float4 v)\n" + "{\n" + " return fast_length(v);\n" + "}\n" + "__inline\n" + "float4 fastNormalize4(float4 v)\n" + "{\n" + " return fast_normalize(v);\n" + "}\n" + "__inline\n" + "float sqrtf(float a)\n" + "{\n" + "// return sqrt(a);\n" + " return native_sqrt(a);\n" + "}\n" + "__inline\n" + "float4 cross3(float4 a1, float4 b1)\n" + "{\n" + " float4 a=make_float4(a1.xyz,0.f);\n" + " float4 b=make_float4(b1.xyz,0.f);\n" + " //float4 a=a1;\n" + " //float4 b=b1;\n" + " return cross(a,b);\n" + "}\n" + "__inline\n" + "float dot3F4(float4 a, float4 b)\n" + "{\n" + " float4 a1 = make_float4(a.xyz,0.f);\n" + " float4 b1 = make_float4(b.xyz,0.f);\n" + " return dot(a1, b1);\n" + "}\n" + "__inline\n" + "float length3(const float4 a)\n" + "{\n" + " return sqrtf(dot3F4(a,a));\n" + "}\n" + "__inline\n" + "float dot4(const float4 a, const float4 b)\n" + "{\n" + " return dot( a, b );\n" + "}\n" + "// for height\n" + "__inline\n" + "float dot3w1(const float4 point, const float4 eqn)\n" + "{\n" + " return dot3F4(point,eqn) + eqn.w;\n" + "}\n" + "__inline\n" + "float4 normalize3(const float4 a)\n" + "{\n" + " float4 n = make_float4(a.x, a.y, a.z, 0.f);\n" + " return fastNormalize4( n );\n" + "// float length = sqrtf(dot3F4(a, a));\n" + "// return 1.f/length * a;\n" + "}\n" + "__inline\n" + "float4 normalize4(const float4 a)\n" + "{\n" + " float length = sqrtf(dot4(a, a));\n" + " return 1.f/length * a;\n" + "}\n" + "__inline\n" + "float4 createEquation(const float4 a, const float4 b, const float4 c)\n" + "{\n" + " float4 eqn;\n" + " float4 ab = b-a;\n" + " float4 ac = c-a;\n" + " eqn = normalize3( cross3(ab, ac) );\n" + " eqn.w = -dot3F4(eqn,a);\n" + " return eqn;\n" + "}\n" + "///////////////////////////////////////\n" + "// Matrix3x3\n" + "///////////////////////////////////////\n" + "typedef struct\n" + "{\n" + " float4 m_row[3];\n" + "}Matrix3x3;\n" + "__inline\n" + "Matrix3x3 mtZero();\n" + "__inline\n" + "Matrix3x3 mtIdentity();\n" + "__inline\n" + "Matrix3x3 mtTranspose(Matrix3x3 m);\n" + "__inline\n" + "Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b);\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b);\n" + "__inline\n" + "Matrix3x3 mtZero()\n" + "{\n" + " Matrix3x3 m;\n" + " m.m_row[0] = (float4)(0.f);\n" + " m.m_row[1] = (float4)(0.f);\n" + " m.m_row[2] = (float4)(0.f);\n" + " return m;\n" + "}\n" + "__inline\n" + "Matrix3x3 mtIdentity()\n" + "{\n" + " Matrix3x3 m;\n" + " m.m_row[0] = (float4)(1,0,0,0);\n" + " m.m_row[1] = (float4)(0,1,0,0);\n" + " m.m_row[2] = (float4)(0,0,1,0);\n" + " return m;\n" + "}\n" + "__inline\n" + "Matrix3x3 mtTranspose(Matrix3x3 m)\n" + "{\n" + " Matrix3x3 out;\n" + " out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" + " out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" + " out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" + " return out;\n" + "}\n" + "__inline\n" + "Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n" + "{\n" + " Matrix3x3 transB;\n" + " transB = mtTranspose( b );\n" + " Matrix3x3 ans;\n" + " // why this doesn't run when 0ing in the for{}\n" + " a.m_row[0].w = 0.f;\n" + " a.m_row[1].w = 0.f;\n" + " a.m_row[2].w = 0.f;\n" + " for(int i=0; i<3; i++)\n" + " {\n" + "// a.m_row[i].w = 0.f;\n" + " ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n" + " ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n" + " ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n" + " ans.m_row[i].w = 0.f;\n" + " }\n" + " return ans;\n" + "}\n" + "__inline\n" + "float4 mtMul1(Matrix3x3 a, float4 b)\n" + "{\n" + " float4 ans;\n" + " ans.x = dot3F4( a.m_row[0], b );\n" + " ans.y = dot3F4( a.m_row[1], b );\n" + " ans.z = dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "float4 mtMul3(float4 a, Matrix3x3 b)\n" + "{\n" + " float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " float4 ans;\n" + " ans.x = dot3F4( a, colx );\n" + " ans.y = dot3F4( a, coly );\n" + " ans.z = dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "///////////////////////////////////////\n" + "// Quaternion\n" + "///////////////////////////////////////\n" + "typedef float4 Quaternion;\n" + "__inline\n" + "Quaternion qtMul(Quaternion a, Quaternion b);\n" + "__inline\n" + "Quaternion qtNormalize(Quaternion in);\n" + "__inline\n" + "float4 qtRotate(Quaternion q, float4 vec);\n" + "__inline\n" + "Quaternion qtInvert(Quaternion q);\n" + "__inline\n" + "Quaternion qtMul(Quaternion a, Quaternion b)\n" + "{\n" + " Quaternion ans;\n" + " ans = cross3( a, b );\n" + " ans += a.w*b+b.w*a;\n" + "// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" + " ans.w = a.w*b.w - dot3F4(a, b);\n" + " return ans;\n" + "}\n" + "__inline\n" + "Quaternion qtNormalize(Quaternion in)\n" + "{\n" + " return fastNormalize4(in);\n" + "// in /= length( in );\n" + "// return in;\n" + "}\n" + "__inline\n" + "float4 qtRotate(Quaternion q, float4 vec)\n" + "{\n" + " Quaternion qInv = qtInvert( q );\n" + " float4 vcpy = vec;\n" + " vcpy.w = 0.f;\n" + " float4 out = qtMul(qtMul(q,vcpy),qInv);\n" + " return out;\n" + "}\n" + "__inline\n" + "Quaternion qtInvert(Quaternion q)\n" + "{\n" + " return (Quaternion)(-q.xyz, q.w);\n" + "}\n" + "__inline\n" + "float4 qtInvRotate(const Quaternion q, float4 vec)\n" + "{\n" + " return qtRotate( qtInvert( q ), vec );\n" + "}\n" + "#define WG_SIZE 64\n" + "typedef struct\n" + "{\n" + " float4 m_pos;\n" + " Quaternion m_quat;\n" + " float4 m_linVel;\n" + " float4 m_angVel;\n" + " u32 m_shapeIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "} Body;\n" + "typedef struct\n" + "{\n" + " Matrix3x3 m_invInertia;\n" + " Matrix3x3 m_initInvInertia;\n" + "} Shape;\n" + "typedef struct\n" + "{\n" + " float4 m_linear;\n" + " float4 m_worldPos[4];\n" + " float4 m_center; \n" + " float m_jacCoeffInv[4];\n" + " float m_b[4];\n" + " float m_appliedRambdaDt[4];\n" + " float m_fJacCoeffInv[2]; \n" + " float m_fAppliedRambdaDt[2]; \n" + " u32 m_bodyA;\n" + " u32 m_bodyB;\n" + " int m_batchIdx;\n" + " u32 m_paddings;\n" + "} Constraint4;\n" + "__kernel void CountBodiesKernel(__global struct b3Contact4Data* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex)\n" + "{\n" + " int i = GET_GLOBAL_IDX;\n" + " \n" + " if( i < numContactManifolds)\n" + " {\n" + " int pa = manifoldPtr[i].m_bodyAPtrAndSignBit;\n" + " bool isFixedA = (pa <0) || (pa == fixedBodyIndex);\n" + " int bodyIndexA = abs(pa);\n" + " if (!isFixedA)\n" + " {\n" + " AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x);\n" + " }\n" + " barrier(CLK_GLOBAL_MEM_FENCE);\n" + " int pb = manifoldPtr[i].m_bodyBPtrAndSignBit;\n" + " bool isFixedB = (pb <0) || (pb == fixedBodyIndex);\n" + " int bodyIndexB = abs(pb);\n" + " if (!isFixedB)\n" + " {\n" + " AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y);\n" + " } \n" + " }\n" + "}\n" + "__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies)\n" + "{\n" + " int i = GET_GLOBAL_IDX;\n" + " \n" + " if( i < numSplitBodies)\n" + " {\n" + " linearVelocities[i] = make_float4(0);\n" + " angularVelocities[i] = make_float4(0);\n" + " }\n" + "}\n" + "__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,\n" + "__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)\n" + "{\n" + " int i = GET_GLOBAL_IDX;\n" + " if (i<numBodies)\n" + " {\n" + " if (gBodies[i].m_invMass)\n" + " {\n" + " int bodyOffset = offsetSplitBodies[i];\n" + " int count = bodyCount[i];\n" + " float factor = 1.f/((float)count);\n" + " float4 averageLinVel = make_float4(0.f);\n" + " float4 averageAngVel = make_float4(0.f);\n" + " \n" + " for (int j=0;j<count;j++)\n" + " {\n" + " averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;\n" + " averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;\n" + " }\n" + " \n" + " for (int j=0;j<count;j++)\n" + " {\n" + " deltaLinearVelocities[bodyOffset+j] = averageLinVel;\n" + " deltaAngularVelocities[bodyOffset+j] = averageAngVel;\n" + " }\n" + " \n" + " }//bodies[i].m_invMass\n" + " }//i<numBodies\n" + "}\n" + "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n" + "{\n" + " *linear = make_float4(n.xyz,0.f);\n" + " *angular0 = cross3(r0, n);\n" + " *angular1 = -cross3(r1, n);\n" + "}\n" + "float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n" + "{\n" + " return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n" + "}\n" + "float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n" + " float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1, float countA, float countB)\n" + "{\n" + " // linear0,1 are normlized\n" + " float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n" + " float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n" + " float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n" + " float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n" + " return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB);\n" + "}\n" + "void btPlaneSpace1 (float4 n, float4* p, float4* q);\n" + " void btPlaneSpace1 (float4 n, float4* p, float4* q)\n" + "{\n" + " if (fabs(n.z) > 0.70710678f) {\n" + " // choose p in y-z plane\n" + " float a = n.y*n.y + n.z*n.z;\n" + " float k = 1.f/sqrt(a);\n" + " p[0].x = 0;\n" + " p[0].y = -n.z*k;\n" + " p[0].z = n.y*k;\n" + " // set q = n x p\n" + " q[0].x = a*k;\n" + " q[0].y = -n.x*p[0].z;\n" + " q[0].z = n.x*p[0].y;\n" + " }\n" + " else {\n" + " // choose p in x-y plane\n" + " float a = n.x*n.x + n.y*n.y;\n" + " float k = 1.f/sqrt(a);\n" + " p[0].x = -n.y*k;\n" + " p[0].y = n.x*k;\n" + " p[0].z = 0;\n" + " // set q = n x p\n" + " q[0].x = -n.z*p[0].y;\n" + " q[0].y = n.z*p[0].x;\n" + " q[0].z = a*k;\n" + " }\n" + "}\n" + "void solveContact(__global Constraint4* cs,\n" + " float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n" + " float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB,\n" + " float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB)\n" + "{\n" + " float minRambdaDt = 0;\n" + " float maxRambdaDt = FLT_MAX;\n" + " for(int ic=0; ic<4; ic++)\n" + " {\n" + " if( cs->m_jacCoeffInv[ic] == 0.f ) continue;\n" + " float4 angular0, angular1, linear;\n" + " float4 r0 = cs->m_worldPos[ic] - posA;\n" + " float4 r1 = cs->m_worldPos[ic] - posB;\n" + " setLinearAndAngular( cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n" + " \n" + " float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n" + " *linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];\n" + " rambdaDt *= cs->m_jacCoeffInv[ic];\n" + " \n" + " {\n" + " float prevSum = cs->m_appliedRambdaDt[ic];\n" + " float updated = prevSum;\n" + " updated += rambdaDt;\n" + " updated = max2( updated, minRambdaDt );\n" + " updated = min2( updated, maxRambdaDt );\n" + " rambdaDt = updated - prevSum;\n" + " cs->m_appliedRambdaDt[ic] = updated;\n" + " }\n" + " \n" + " float4 linImp0 = invMassA*linear*rambdaDt;\n" + " float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" + " float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" + " float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" + " \n" + " if (invMassA)\n" + " {\n" + " *dLinVelA += linImp0;\n" + " *dAngVelA += angImp0;\n" + " }\n" + " if (invMassB)\n" + " {\n" + " *dLinVelB += linImp1;\n" + " *dAngVelB += angImp1;\n" + " }\n" + " }\n" + "}\n" + "// solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n" + "void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, \n" + "__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n" + "__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n" + "{\n" + " //float frictionCoeff = ldsCs[0].m_linear.w;\n" + " int aIdx = ldsCs[0].m_bodyA;\n" + " int bIdx = ldsCs[0].m_bodyB;\n" + " float4 posA = gBodies[aIdx].m_pos;\n" + " float4 linVelA = gBodies[aIdx].m_linVel;\n" + " float4 angVelA = gBodies[aIdx].m_angVel;\n" + " float invMassA = gBodies[aIdx].m_invMass;\n" + " Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" + " float4 posB = gBodies[bIdx].m_pos;\n" + " float4 linVelB = gBodies[bIdx].m_linVel;\n" + " float4 angVelB = gBodies[bIdx].m_angVel;\n" + " float invMassB = gBodies[bIdx].m_invMass;\n" + " Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" + " \n" + " float4 dLinVelA = make_float4(0,0,0,0);\n" + " float4 dAngVelA = make_float4(0,0,0,0);\n" + " float4 dLinVelB = make_float4(0,0,0,0);\n" + " float4 dAngVelB = make_float4(0,0,0,0);\n" + " \n" + " int bodyOffsetA = offsetSplitBodies[aIdx];\n" + " int constraintOffsetA = contactConstraintOffsets[0].x;\n" + " int splitIndexA = bodyOffsetA+constraintOffsetA;\n" + " \n" + " if (invMassA)\n" + " {\n" + " dLinVelA = deltaLinearVelocities[splitIndexA];\n" + " dAngVelA = deltaAngularVelocities[splitIndexA];\n" + " }\n" + " int bodyOffsetB = offsetSplitBodies[bIdx];\n" + " int constraintOffsetB = contactConstraintOffsets[0].y;\n" + " int splitIndexB= bodyOffsetB+constraintOffsetB;\n" + " if (invMassB)\n" + " {\n" + " dLinVelB = deltaLinearVelocities[splitIndexB];\n" + " dAngVelB = deltaAngularVelocities[splitIndexB];\n" + " }\n" + " solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" + " posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB);\n" + " if (invMassA)\n" + " {\n" + " deltaLinearVelocities[splitIndexA] = dLinVelA;\n" + " deltaAngularVelocities[splitIndexA] = dAngVelA;\n" + " } \n" + " if (invMassB)\n" + " {\n" + " deltaLinearVelocities[splitIndexB] = dLinVelB;\n" + " deltaAngularVelocities[splitIndexB] = dAngVelB;\n" + " }\n" + "}\n" + "__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n" + "__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n" + "float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n" + ")\n" + "{\n" + " int i = GET_GLOBAL_IDX;\n" + " if (i<numManifolds)\n" + " {\n" + " solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n" + " }\n" + "}\n" + "void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs,\n" + " __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n" + " __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n" + "{\n" + " float frictionCoeff = 0.7f;//ldsCs[0].m_linear.w;\n" + " int aIdx = ldsCs[0].m_bodyA;\n" + " int bIdx = ldsCs[0].m_bodyB;\n" + " float4 posA = gBodies[aIdx].m_pos;\n" + " float4 linVelA = gBodies[aIdx].m_linVel;\n" + " float4 angVelA = gBodies[aIdx].m_angVel;\n" + " float invMassA = gBodies[aIdx].m_invMass;\n" + " Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" + " float4 posB = gBodies[bIdx].m_pos;\n" + " float4 linVelB = gBodies[bIdx].m_linVel;\n" + " float4 angVelB = gBodies[bIdx].m_angVel;\n" + " float invMassB = gBodies[bIdx].m_invMass;\n" + " Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" + " \n" + " float4 dLinVelA = make_float4(0,0,0,0);\n" + " float4 dAngVelA = make_float4(0,0,0,0);\n" + " float4 dLinVelB = make_float4(0,0,0,0);\n" + " float4 dAngVelB = make_float4(0,0,0,0);\n" + " \n" + " int bodyOffsetA = offsetSplitBodies[aIdx];\n" + " int constraintOffsetA = contactConstraintOffsets[0].x;\n" + " int splitIndexA = bodyOffsetA+constraintOffsetA;\n" + " \n" + " if (invMassA)\n" + " {\n" + " dLinVelA = deltaLinearVelocities[splitIndexA];\n" + " dAngVelA = deltaAngularVelocities[splitIndexA];\n" + " }\n" + " int bodyOffsetB = offsetSplitBodies[bIdx];\n" + " int constraintOffsetB = contactConstraintOffsets[0].y;\n" + " int splitIndexB= bodyOffsetB+constraintOffsetB;\n" + " if (invMassB)\n" + " {\n" + " dLinVelB = deltaLinearVelocities[splitIndexB];\n" + " dAngVelB = deltaAngularVelocities[splitIndexB];\n" + " }\n" + " {\n" + " float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};\n" + " float minRambdaDt[4] = {0.f,0.f,0.f,0.f};\n" + " float sum = 0;\n" + " for(int j=0; j<4; j++)\n" + " {\n" + " sum +=ldsCs[0].m_appliedRambdaDt[j];\n" + " }\n" + " frictionCoeff = 0.7f;\n" + " for(int j=0; j<4; j++)\n" + " {\n" + " maxRambdaDt[j] = frictionCoeff*sum;\n" + " minRambdaDt[j] = -maxRambdaDt[j];\n" + " }\n" + " \n" + "// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n" + "// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );\n" + " \n" + " \n" + " {\n" + " \n" + " __global Constraint4* cs = ldsCs;\n" + " \n" + " if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n" + " const float4 center = cs->m_center;\n" + " \n" + " float4 n = -cs->m_linear;\n" + " \n" + " float4 tangent[2];\n" + " btPlaneSpace1(n,&tangent[0],&tangent[1]);\n" + " float4 angular0, angular1, linear;\n" + " float4 r0 = center - posA;\n" + " float4 r1 = center - posB;\n" + " for(int i=0; i<2; i++)\n" + " {\n" + " setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n" + " float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n" + " linVelA+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB );\n" + " rambdaDt *= cs->m_fJacCoeffInv[i];\n" + " \n" + " {\n" + " float prevSum = cs->m_fAppliedRambdaDt[i];\n" + " float updated = prevSum;\n" + " updated += rambdaDt;\n" + " updated = max2( updated, minRambdaDt[i] );\n" + " updated = min2( updated, maxRambdaDt[i] );\n" + " rambdaDt = updated - prevSum;\n" + " cs->m_fAppliedRambdaDt[i] = updated;\n" + " }\n" + " \n" + " float4 linImp0 = invMassA*linear*rambdaDt;\n" + " float4 linImp1 = invMassB*(-linear)*rambdaDt;\n" + " float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n" + " float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n" + " \n" + " dLinVelA += linImp0;\n" + " dAngVelA += angImp0;\n" + " dLinVelB += linImp1;\n" + " dAngVelB += angImp1;\n" + " }\n" + " { // angular damping for point constraint\n" + " float4 ab = normalize3( posB - posA );\n" + " float4 ac = normalize3( center - posA );\n" + " if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))\n" + " {\n" + " float angNA = dot3F4( n, angVelA );\n" + " float angNB = dot3F4( n, angVelB );\n" + " \n" + " dAngVelA -= (angNA*0.1f)*n;\n" + " dAngVelB -= (angNB*0.1f)*n;\n" + " }\n" + " }\n" + " }\n" + " \n" + " \n" + " }\n" + " if (invMassA)\n" + " {\n" + " deltaLinearVelocities[splitIndexA] = dLinVelA;\n" + " deltaAngularVelocities[splitIndexA] = dAngVelA;\n" + " } \n" + " if (invMassB)\n" + " {\n" + " deltaLinearVelocities[splitIndexB] = dLinVelB;\n" + " deltaAngularVelocities[splitIndexB] = dAngVelB;\n" + " }\n" + " \n" + "}\n" + "__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n" + " __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n" + " __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n" + " float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n" + ")\n" + "{\n" + " int i = GET_GLOBAL_IDX;\n" + " if (i<numManifolds)\n" + " {\n" + " solveFrictionConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n" + " }\n" + "}\n" + "__kernel void UpdateBodyVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,\n" + " __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)\n" + "{\n" + " int i = GET_GLOBAL_IDX;\n" + " if (i<numBodies)\n" + " {\n" + " if (gBodies[i].m_invMass)\n" + " {\n" + " int bodyOffset = offsetSplitBodies[i];\n" + " int count = bodyCount[i];\n" + " if (count)\n" + " {\n" + " gBodies[i].m_linVel += deltaLinearVelocities[bodyOffset];\n" + " gBodies[i].m_angVel += deltaAngularVelocities[bodyOffset];\n" + " }\n" + " }\n" + " }\n" + "}\n" + "void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVelA, float invMassA, const Matrix3x3 invInertiaA,\n" + " const float4 posB, const float4 linVelB, const float4 angVelB, float invMassB, const Matrix3x3 invInertiaB, \n" + " __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,float countA, float countB,\n" + " Constraint4* dstC )\n" + "{\n" + " dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);\n" + " dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n" + " float dtInv = 1.f/dt;\n" + " for(int ic=0; ic<4; ic++)\n" + " {\n" + " dstC->m_appliedRambdaDt[ic] = 0.f;\n" + " }\n" + " dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n" + " dstC->m_linear = src->m_worldNormalOnB;\n" + " dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n" + " for(int ic=0; ic<4; ic++)\n" + " {\n" + " float4 r0 = src->m_worldPosB[ic] - posA;\n" + " float4 r1 = src->m_worldPosB[ic] - posB;\n" + " if( ic >= src->m_worldNormalOnB.w )//npoints\n" + " {\n" + " dstC->m_jacCoeffInv[ic] = 0.f;\n" + " continue;\n" + " }\n" + " float relVelN;\n" + " {\n" + " float4 linear, angular0, angular1;\n" + " setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);\n" + " dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n" + " invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB);\n" + " relVelN = calcRelVel(linear, -linear, angular0, angular1,\n" + " linVelA, angVelA, linVelB, angVelB);\n" + " float e = 0.f;//src->getRestituitionCoeff();\n" + " if( relVelN*relVelN < 0.004f ) e = 0.f;\n" + " dstC->m_b[ic] = e*relVelN;\n" + " //float penetration = src->m_worldPosB[ic].w;\n" + " dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n" + " dstC->m_appliedRambdaDt[ic] = 0.f;\n" + " }\n" + " }\n" + " if( src->m_worldNormalOnB.w > 0 )//npoints\n" + " { // prepare friction\n" + " float4 center = make_float4(0.f);\n" + " for(int i=0; i<src->m_worldNormalOnB.w; i++) \n" + " center += src->m_worldPosB[i];\n" + " center /= (float)src->m_worldNormalOnB.w;\n" + " float4 tangent[2];\n" + " btPlaneSpace1(-src->m_worldNormalOnB,&tangent[0],&tangent[1]);\n" + " \n" + " float4 r[2];\n" + " r[0] = center - posA;\n" + " r[1] = center - posB;\n" + " for(int i=0; i<2; i++)\n" + " {\n" + " float4 linear, angular0, angular1;\n" + " setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n" + " dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n" + " invMassA, &invInertiaA, invMassB, &invInertiaB ,countA, countB);\n" + " dstC->m_fAppliedRambdaDt[i] = 0.f;\n" + " }\n" + " dstC->m_center = center;\n" + " }\n" + " for(int i=0; i<4; i++)\n" + " {\n" + " if( i<src->m_worldNormalOnB.w )\n" + " {\n" + " dstC->m_worldPos[i] = src->m_worldPosB[i];\n" + " }\n" + " else\n" + " {\n" + " dstC->m_worldPos[i] = make_float4(0.f);\n" + " }\n" + " }\n" + "}\n" + "__kernel\n" + "__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n" + "void ContactToConstraintSplitKernel(__global const struct b3Contact4Data* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, \n" + "__global const unsigned int* bodyCount,\n" + "int nContacts,\n" + "float dt,\n" + "float positionDrift,\n" + "float positionConstraintCoeff\n" + ")\n" + "{\n" + " int gIdx = GET_GLOBAL_IDX;\n" + " \n" + " if( gIdx < nContacts )\n" + " {\n" + " int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n" + " int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n" + " float4 posA = gBodies[aIdx].m_pos;\n" + " float4 linVelA = gBodies[aIdx].m_linVel;\n" + " float4 angVelA = gBodies[aIdx].m_angVel;\n" + " float invMassA = gBodies[aIdx].m_invMass;\n" + " Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n" + " float4 posB = gBodies[bIdx].m_pos;\n" + " float4 linVelB = gBodies[bIdx].m_linVel;\n" + " float4 angVelB = gBodies[bIdx].m_angVel;\n" + " float invMassB = gBodies[bIdx].m_invMass;\n" + " Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n" + " Constraint4 cs;\n" + " float countA = invMassA != 0.f ? (float)bodyCount[aIdx] : 1;\n" + " float countB = invMassB != 0.f ? (float)bodyCount[bIdx] : 1;\n" + " setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n" + " &gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB,\n" + " &cs );\n" + " \n" + " cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n" + " gConstraintOut[gIdx] = cs;\n" + " }\n" + "}\n"; diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h index d70e74017a..bb949b2027 100644 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h @@ -1,483 +1,482 @@ //this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project -static const char* updateAabbsKernelCL= \ -"#ifndef B3_UPDATE_AABBS_H\n" -"#define B3_UPDATE_AABBS_H\n" -"#ifndef B3_AABB_H\n" -"#define B3_AABB_H\n" -"#ifndef B3_FLOAT4_H\n" -"#define B3_FLOAT4_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#define B3_PLATFORM_DEFINITIONS_H\n" -"struct MyTest\n" -"{\n" -" int bla;\n" -"};\n" -"#ifdef __cplusplus\n" -"#else\n" -"//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" -"#define B3_LARGE_FLOAT 1e18f\n" -"#define B3_INFINITY 1e18f\n" -"#define b3Assert(a)\n" -"#define b3ConstArray(a) __global const a*\n" -"#define b3AtomicInc atomic_inc\n" -"#define b3AtomicAdd atomic_add\n" -"#define b3Fabs fabs\n" -"#define b3Sqrt native_sqrt\n" -"#define b3Sin native_sin\n" -"#define b3Cos native_cos\n" -"#define B3_STATIC\n" -"#endif\n" -"#endif\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Float4;\n" -" #define b3Float4ConstArg const b3Float4\n" -" #define b3MakeFloat4 (float4)\n" -" float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return dot(a1, b1);\n" -" }\n" -" b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" -" {\n" -" float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" -" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" -" return cross(a1, b1);\n" -" }\n" -" #define b3MinFloat4 min\n" -" #define b3MaxFloat4 max\n" -" #define b3Normalized(a) normalize(a)\n" -"#endif \n" -" \n" -"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" -"{\n" -" if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" -" return false;\n" -" return true;\n" -"}\n" -"inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" -"{\n" -" float maxDot = -B3_INFINITY;\n" -" int i = 0;\n" -" int ptIndex = -1;\n" -" for( i = 0; i < vecLen; i++ )\n" -" {\n" -" float dot = b3Dot3F4(vecArray[i],vec);\n" -" \n" -" if( dot > maxDot )\n" -" {\n" -" maxDot = dot;\n" -" ptIndex = i;\n" -" }\n" -" }\n" -" b3Assert(ptIndex>=0);\n" -" if (ptIndex<0)\n" -" {\n" -" ptIndex = 0;\n" -" }\n" -" *dotOut = maxDot;\n" -" return ptIndex;\n" -"}\n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_MAT3x3_H\n" -"#define B3_MAT3x3_H\n" -"#ifndef B3_QUAT_H\n" -"#define B3_QUAT_H\n" -"#ifndef B3_PLATFORM_DEFINITIONS_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif\n" -"#endif\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -" typedef float4 b3Quat;\n" -" #define b3QuatConstArg const b3Quat\n" -" \n" -" \n" -"inline float4 b3FastNormalize4(float4 v)\n" -"{\n" -" v = (float4)(v.xyz,0.f);\n" -" return fast_normalize(v);\n" -"}\n" -" \n" -"inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" -"inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" -"inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" -"inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" -"inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" -"inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" -"{\n" -" b3Quat ans;\n" -" ans = b3Cross3( a, b );\n" -" ans += a.w*b+b.w*a;\n" -"// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" -" ans.w = a.w*b.w - b3Dot3F4(a, b);\n" -" return ans;\n" -"}\n" -"inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" -"{\n" -" b3Quat q;\n" -" q=in;\n" -" //return b3FastNormalize4(in);\n" -" float len = native_sqrt(dot(q, q));\n" -" if(len > 0.f)\n" -" {\n" -" q *= 1.f / len;\n" -" }\n" -" else\n" -" {\n" -" q.x = q.y = q.z = 0.f;\n" -" q.w = 1.f;\n" -" }\n" -" return q;\n" -"}\n" -"inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" -"{\n" -" b3Quat qInv = b3QuatInvert( q );\n" -" float4 vcpy = vec;\n" -" vcpy.w = 0.f;\n" -" float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" -" return out;\n" -"}\n" -"inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" -"{\n" -" return (b3Quat)(-q.xyz, q.w);\n" -"}\n" -"inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" -"{\n" -" return (b3Quat)(-q.xyz, q.w);\n" -"}\n" -"inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" -"{\n" -" return b3QuatRotate( b3QuatInvert( q ), vec );\n" -"}\n" -"inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" -"{\n" -" return b3QuatRotate( orientation, point ) + (translation);\n" -"}\n" -" \n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"typedef struct\n" -"{\n" -" b3Float4 m_row[3];\n" -"}b3Mat3x3;\n" -"#define b3Mat3x3ConstArg const b3Mat3x3\n" -"#define b3GetRow(m,row) (m.m_row[row])\n" -"inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" -"{\n" -" b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" -" b3Mat3x3 out;\n" -" out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" -" out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" -" out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" -" out.m_row[0].w = 0.f;\n" -" out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" -" out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" -" out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" -" out.m_row[1].w = 0.f;\n" -" out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" -" out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" -" out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" -" out.m_row[2].w = 0.f;\n" -" return out;\n" -"}\n" -"inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" -"{\n" -" b3Mat3x3 out;\n" -" out.m_row[0] = fabs(matIn.m_row[0]);\n" -" out.m_row[1] = fabs(matIn.m_row[1]);\n" -" out.m_row[2] = fabs(matIn.m_row[2]);\n" -" return out;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtZero();\n" -"__inline\n" -"b3Mat3x3 mtIdentity();\n" -"__inline\n" -"b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" -"__inline\n" -"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" -"__inline\n" -"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" -"__inline\n" -"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" -"__inline\n" -"b3Mat3x3 mtZero()\n" -"{\n" -" b3Mat3x3 m;\n" -" m.m_row[0] = (b3Float4)(0.f);\n" -" m.m_row[1] = (b3Float4)(0.f);\n" -" m.m_row[2] = (b3Float4)(0.f);\n" -" return m;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtIdentity()\n" -"{\n" -" b3Mat3x3 m;\n" -" m.m_row[0] = (b3Float4)(1,0,0,0);\n" -" m.m_row[1] = (b3Float4)(0,1,0,0);\n" -" m.m_row[2] = (b3Float4)(0,0,1,0);\n" -" return m;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" -"{\n" -" b3Mat3x3 out;\n" -" out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" -" out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" -" out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" -" return out;\n" -"}\n" -"__inline\n" -"b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" -"{\n" -" b3Mat3x3 transB;\n" -" transB = mtTranspose( b );\n" -" b3Mat3x3 ans;\n" -" // why this doesn't run when 0ing in the for{}\n" -" a.m_row[0].w = 0.f;\n" -" a.m_row[1].w = 0.f;\n" -" a.m_row[2].w = 0.f;\n" -" for(int i=0; i<3; i++)\n" -" {\n" -"// a.m_row[i].w = 0.f;\n" -" ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" -" ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" -" ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" -" ans.m_row[i].w = 0.f;\n" -" }\n" -" return ans;\n" -"}\n" -"__inline\n" -"b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" -"{\n" -" b3Float4 ans;\n" -" ans.x = b3Dot3F4( a.m_row[0], b );\n" -" ans.y = b3Dot3F4( a.m_row[1], b );\n" -" ans.z = b3Dot3F4( a.m_row[2], b );\n" -" ans.w = 0.f;\n" -" return ans;\n" -"}\n" -"__inline\n" -"b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" -"{\n" -" b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" -" b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" -" b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" -" b3Float4 ans;\n" -" ans.x = b3Dot3F4( a, colx );\n" -" ans.y = b3Dot3F4( a, coly );\n" -" ans.z = b3Dot3F4( a, colz );\n" -" return ans;\n" -"}\n" -"#endif\n" -"#endif //B3_MAT3x3_H\n" -"typedef struct b3Aabb b3Aabb_t;\n" -"struct b3Aabb\n" -"{\n" -" union\n" -" {\n" -" float m_min[4];\n" -" b3Float4 m_minVec;\n" -" int m_minIndices[4];\n" -" };\n" -" union\n" -" {\n" -" float m_max[4];\n" -" b3Float4 m_maxVec;\n" -" int m_signedMaxIndices[4];\n" -" };\n" -"};\n" -"inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,\n" -" b3Float4ConstArg pos,\n" -" b3QuatConstArg orn,\n" -" b3Float4* aabbMinOut,b3Float4* aabbMaxOut)\n" -"{\n" -" b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);\n" -" localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);\n" -" b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);\n" -" b3Mat3x3 m;\n" -" m = b3QuatGetRotationMatrix(orn);\n" -" b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);\n" -" b3Float4 center = b3TransformPoint(localCenter,pos,orn);\n" -" \n" -" b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),\n" -" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),\n" -" b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),\n" -" 0.f);\n" -" *aabbMinOut = center-extent;\n" -" *aabbMaxOut = center+extent;\n" -"}\n" -"/// conservative test for overlap between two aabbs\n" -"inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,\n" -" b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)\n" -"{\n" -" bool overlap = true;\n" -" overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;\n" -" overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;\n" -" overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;\n" -" return overlap;\n" -"}\n" -"#endif //B3_AABB_H\n" -"#ifndef B3_COLLIDABLE_H\n" -"#define B3_COLLIDABLE_H\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"enum b3ShapeTypes\n" -"{\n" -" SHAPE_HEIGHT_FIELD=1,\n" -" SHAPE_CONVEX_HULL=3,\n" -" SHAPE_PLANE=4,\n" -" SHAPE_CONCAVE_TRIMESH=5,\n" -" SHAPE_COMPOUND_OF_CONVEX_HULLS=6,\n" -" SHAPE_SPHERE=7,\n" -" MAX_NUM_SHAPE_TYPES,\n" -"};\n" -"typedef struct b3Collidable b3Collidable_t;\n" -"struct b3Collidable\n" -"{\n" -" union {\n" -" int m_numChildShapes;\n" -" int m_bvhIndex;\n" -" };\n" -" union\n" -" {\n" -" float m_radius;\n" -" int m_compoundBvhIndex;\n" -" };\n" -" int m_shapeType;\n" -" union\n" -" {\n" -" int m_shapeIndex;\n" -" float m_height;\n" -" };\n" -"};\n" -"typedef struct b3GpuChildShape b3GpuChildShape_t;\n" -"struct b3GpuChildShape\n" -"{\n" -" b3Float4 m_childPosition;\n" -" b3Quat m_childOrientation;\n" -" union\n" -" {\n" -" int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS\n" -" int m_capsuleAxis;\n" -" };\n" -" union \n" -" {\n" -" float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES\n" -" int m_numChildShapes;//used for compound shape\n" -" };\n" -" union \n" -" {\n" -" float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES\n" -" int m_collidableShapeIndex;\n" -" };\n" -" int m_shapeType;\n" -"};\n" -"struct b3CompoundOverlappingPair\n" -"{\n" -" int m_bodyIndexA;\n" -" int m_bodyIndexB;\n" -"// int m_pairType;\n" -" int m_childShapeIndexA;\n" -" int m_childShapeIndexB;\n" -"};\n" -"#endif //B3_COLLIDABLE_H\n" -"#ifndef B3_RIGIDBODY_DATA_H\n" -"#define B3_RIGIDBODY_DATA_H\n" -"#ifndef B3_FLOAT4_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_FLOAT4_H\n" -"#ifndef B3_QUAT_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif \n" -"#endif //B3_QUAT_H\n" -"#ifndef B3_MAT3x3_H\n" -"#ifdef __cplusplus\n" -"#else\n" -"#endif\n" -"#endif //B3_MAT3x3_H\n" -"typedef struct b3RigidBodyData b3RigidBodyData_t;\n" -"struct b3RigidBodyData\n" -"{\n" -" b3Float4 m_pos;\n" -" b3Quat m_quat;\n" -" b3Float4 m_linVel;\n" -" b3Float4 m_angVel;\n" -" int m_collidableIdx;\n" -" float m_invMass;\n" -" float m_restituitionCoeff;\n" -" float m_frictionCoeff;\n" -"};\n" -"typedef struct b3InertiaData b3InertiaData_t;\n" -"struct b3InertiaData\n" -"{\n" -" b3Mat3x3 m_invInertiaWorld;\n" -" b3Mat3x3 m_initInvInertia;\n" -"};\n" -"#endif //B3_RIGIDBODY_DATA_H\n" -" \n" -"void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)\n" -"{\n" -" __global const b3RigidBodyData_t* body = &bodies[bodyId];\n" -" b3Float4 position = body->m_pos;\n" -" b3Quat orientation = body->m_quat;\n" -" \n" -" int collidableIndex = body->m_collidableIdx;\n" -" int shapeIndex = collidables[collidableIndex].m_shapeIndex;\n" -" \n" -" if (shapeIndex>=0)\n" -" {\n" -" \n" -" b3Aabb_t localAabb = localShapeAABB[collidableIndex];\n" -" b3Aabb_t worldAabb;\n" -" \n" -" b3Float4 aabbAMinOut,aabbAMaxOut; \n" -" float margin = 0.f;\n" -" b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut);\n" -" \n" -" worldAabb.m_minVec =aabbAMinOut;\n" -" worldAabb.m_minIndices[3] = bodyId;\n" -" worldAabb.m_maxVec = aabbAMaxOut;\n" -" worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1;\n" -" worldAabbs[bodyId] = worldAabb;\n" -" }\n" -"}\n" -"#endif //B3_UPDATE_AABBS_H\n" -"__kernel void initializeGpuAabbsFull( const int numNodes, __global b3RigidBodyData_t* gBodies,__global b3Collidable_t* collidables, __global b3Aabb_t* plocalShapeAABB, __global b3Aabb_t* pAABB)\n" -"{\n" -" int nodeID = get_global_id(0);\n" -" if( nodeID < numNodes )\n" -" {\n" -" b3ComputeWorldAabb(nodeID, gBodies, collidables, plocalShapeAABB,pAABB);\n" -" }\n" -"}\n" -"__kernel void clearOverlappingPairsKernel( __global int4* pairs, int numPairs)\n" -"{\n" -" int pairId = get_global_id(0);\n" -" if( pairId< numPairs )\n" -" {\n" -" pairs[pairId].z = 0xffffffff;\n" -" }\n" -"}\n" -; +static const char* updateAabbsKernelCL = + "#ifndef B3_UPDATE_AABBS_H\n" + "#define B3_UPDATE_AABBS_H\n" + "#ifndef B3_AABB_H\n" + "#define B3_AABB_H\n" + "#ifndef B3_FLOAT4_H\n" + "#define B3_FLOAT4_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#define B3_PLATFORM_DEFINITIONS_H\n" + "struct MyTest\n" + "{\n" + " int bla;\n" + "};\n" + "#ifdef __cplusplus\n" + "#else\n" + "//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX\n" + "#define B3_LARGE_FLOAT 1e18f\n" + "#define B3_INFINITY 1e18f\n" + "#define b3Assert(a)\n" + "#define b3ConstArray(a) __global const a*\n" + "#define b3AtomicInc atomic_inc\n" + "#define b3AtomicAdd atomic_add\n" + "#define b3Fabs fabs\n" + "#define b3Sqrt native_sqrt\n" + "#define b3Sin native_sin\n" + "#define b3Cos native_cos\n" + "#define B3_STATIC\n" + "#endif\n" + "#endif\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Float4;\n" + " #define b3Float4ConstArg const b3Float4\n" + " #define b3MakeFloat4 (float4)\n" + " float b3Dot3F4(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return dot(a1, b1);\n" + " }\n" + " b3Float4 b3Cross3(b3Float4ConstArg v0,b3Float4ConstArg v1)\n" + " {\n" + " float4 a1 = b3MakeFloat4(v0.xyz,0.f);\n" + " float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n" + " return cross(a1, b1);\n" + " }\n" + " #define b3MinFloat4 min\n" + " #define b3MaxFloat4 max\n" + " #define b3Normalized(a) normalize(a)\n" + "#endif \n" + " \n" + "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n" + "{\n" + " if(b3Fabs(v.x)>1e-6 || b3Fabs(v.y)>1e-6 || b3Fabs(v.z)>1e-6) \n" + " return false;\n" + " return true;\n" + "}\n" + "inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut )\n" + "{\n" + " float maxDot = -B3_INFINITY;\n" + " int i = 0;\n" + " int ptIndex = -1;\n" + " for( i = 0; i < vecLen; i++ )\n" + " {\n" + " float dot = b3Dot3F4(vecArray[i],vec);\n" + " \n" + " if( dot > maxDot )\n" + " {\n" + " maxDot = dot;\n" + " ptIndex = i;\n" + " }\n" + " }\n" + " b3Assert(ptIndex>=0);\n" + " if (ptIndex<0)\n" + " {\n" + " ptIndex = 0;\n" + " }\n" + " *dotOut = maxDot;\n" + " return ptIndex;\n" + "}\n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_MAT3x3_H\n" + "#define B3_MAT3x3_H\n" + "#ifndef B3_QUAT_H\n" + "#define B3_QUAT_H\n" + "#ifndef B3_PLATFORM_DEFINITIONS_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif\n" + "#endif\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + " typedef float4 b3Quat;\n" + " #define b3QuatConstArg const b3Quat\n" + " \n" + " \n" + "inline float4 b3FastNormalize4(float4 v)\n" + "{\n" + " v = (float4)(v.xyz,0.f);\n" + " return fast_normalize(v);\n" + "}\n" + " \n" + "inline b3Quat b3QuatMul(b3Quat a, b3Quat b);\n" + "inline b3Quat b3QuatNormalized(b3QuatConstArg in);\n" + "inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);\n" + "inline b3Quat b3QuatInvert(b3QuatConstArg q);\n" + "inline b3Quat b3QuatInverse(b3QuatConstArg q);\n" + "inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)\n" + "{\n" + " b3Quat ans;\n" + " ans = b3Cross3( a, b );\n" + " ans += a.w*b+b.w*a;\n" + "// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n" + " ans.w = a.w*b.w - b3Dot3F4(a, b);\n" + " return ans;\n" + "}\n" + "inline b3Quat b3QuatNormalized(b3QuatConstArg in)\n" + "{\n" + " b3Quat q;\n" + " q=in;\n" + " //return b3FastNormalize4(in);\n" + " float len = native_sqrt(dot(q, q));\n" + " if(len > 0.f)\n" + " {\n" + " q *= 1.f / len;\n" + " }\n" + " else\n" + " {\n" + " q.x = q.y = q.z = 0.f;\n" + " q.w = 1.f;\n" + " }\n" + " return q;\n" + "}\n" + "inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" + "{\n" + " b3Quat qInv = b3QuatInvert( q );\n" + " float4 vcpy = vec;\n" + " vcpy.w = 0.f;\n" + " float4 out = b3QuatMul(b3QuatMul(q,vcpy),qInv);\n" + " return out;\n" + "}\n" + "inline b3Quat b3QuatInverse(b3QuatConstArg q)\n" + "{\n" + " return (b3Quat)(-q.xyz, q.w);\n" + "}\n" + "inline b3Quat b3QuatInvert(b3QuatConstArg q)\n" + "{\n" + " return (b3Quat)(-q.xyz, q.w);\n" + "}\n" + "inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)\n" + "{\n" + " return b3QuatRotate( b3QuatInvert( q ), vec );\n" + "}\n" + "inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)\n" + "{\n" + " return b3QuatRotate( orientation, point ) + (translation);\n" + "}\n" + " \n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "typedef struct\n" + "{\n" + " b3Float4 m_row[3];\n" + "}b3Mat3x3;\n" + "#define b3Mat3x3ConstArg const b3Mat3x3\n" + "#define b3GetRow(m,row) (m.m_row[row])\n" + "inline b3Mat3x3 b3QuatGetRotationMatrix(b3Quat quat)\n" + "{\n" + " b3Float4 quat2 = (b3Float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n" + " b3Mat3x3 out;\n" + " out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n" + " out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n" + " out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n" + " out.m_row[0].w = 0.f;\n" + " out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n" + " out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n" + " out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n" + " out.m_row[1].w = 0.f;\n" + " out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n" + " out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n" + " out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n" + " out.m_row[2].w = 0.f;\n" + " return out;\n" + "}\n" + "inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)\n" + "{\n" + " b3Mat3x3 out;\n" + " out.m_row[0] = fabs(matIn.m_row[0]);\n" + " out.m_row[1] = fabs(matIn.m_row[1]);\n" + " out.m_row[2] = fabs(matIn.m_row[2]);\n" + " return out;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtZero();\n" + "__inline\n" + "b3Mat3x3 mtIdentity();\n" + "__inline\n" + "b3Mat3x3 mtTranspose(b3Mat3x3 m);\n" + "__inline\n" + "b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);\n" + "__inline\n" + "b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);\n" + "__inline\n" + "b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);\n" + "__inline\n" + "b3Mat3x3 mtZero()\n" + "{\n" + " b3Mat3x3 m;\n" + " m.m_row[0] = (b3Float4)(0.f);\n" + " m.m_row[1] = (b3Float4)(0.f);\n" + " m.m_row[2] = (b3Float4)(0.f);\n" + " return m;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtIdentity()\n" + "{\n" + " b3Mat3x3 m;\n" + " m.m_row[0] = (b3Float4)(1,0,0,0);\n" + " m.m_row[1] = (b3Float4)(0,1,0,0);\n" + " m.m_row[2] = (b3Float4)(0,0,1,0);\n" + " return m;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtTranspose(b3Mat3x3 m)\n" + "{\n" + " b3Mat3x3 out;\n" + " out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n" + " out.m_row[1] = (b3Float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n" + " out.m_row[2] = (b3Float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n" + " return out;\n" + "}\n" + "__inline\n" + "b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)\n" + "{\n" + " b3Mat3x3 transB;\n" + " transB = mtTranspose( b );\n" + " b3Mat3x3 ans;\n" + " // why this doesn't run when 0ing in the for{}\n" + " a.m_row[0].w = 0.f;\n" + " a.m_row[1].w = 0.f;\n" + " a.m_row[2].w = 0.f;\n" + " for(int i=0; i<3; i++)\n" + " {\n" + "// a.m_row[i].w = 0.f;\n" + " ans.m_row[i].x = b3Dot3F4(a.m_row[i],transB.m_row[0]);\n" + " ans.m_row[i].y = b3Dot3F4(a.m_row[i],transB.m_row[1]);\n" + " ans.m_row[i].z = b3Dot3F4(a.m_row[i],transB.m_row[2]);\n" + " ans.m_row[i].w = 0.f;\n" + " }\n" + " return ans;\n" + "}\n" + "__inline\n" + "b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)\n" + "{\n" + " b3Float4 ans;\n" + " ans.x = b3Dot3F4( a.m_row[0], b );\n" + " ans.y = b3Dot3F4( a.m_row[1], b );\n" + " ans.z = b3Dot3F4( a.m_row[2], b );\n" + " ans.w = 0.f;\n" + " return ans;\n" + "}\n" + "__inline\n" + "b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)\n" + "{\n" + " b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n" + " b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n" + " b3Float4 colz = b3MakeFloat4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n" + " b3Float4 ans;\n" + " ans.x = b3Dot3F4( a, colx );\n" + " ans.y = b3Dot3F4( a, coly );\n" + " ans.z = b3Dot3F4( a, colz );\n" + " return ans;\n" + "}\n" + "#endif\n" + "#endif //B3_MAT3x3_H\n" + "typedef struct b3Aabb b3Aabb_t;\n" + "struct b3Aabb\n" + "{\n" + " union\n" + " {\n" + " float m_min[4];\n" + " b3Float4 m_minVec;\n" + " int m_minIndices[4];\n" + " };\n" + " union\n" + " {\n" + " float m_max[4];\n" + " b3Float4 m_maxVec;\n" + " int m_signedMaxIndices[4];\n" + " };\n" + "};\n" + "inline void b3TransformAabb2(b3Float4ConstArg localAabbMin,b3Float4ConstArg localAabbMax, float margin,\n" + " b3Float4ConstArg pos,\n" + " b3QuatConstArg orn,\n" + " b3Float4* aabbMinOut,b3Float4* aabbMaxOut)\n" + "{\n" + " b3Float4 localHalfExtents = 0.5f*(localAabbMax-localAabbMin);\n" + " localHalfExtents+=b3MakeFloat4(margin,margin,margin,0.f);\n" + " b3Float4 localCenter = 0.5f*(localAabbMax+localAabbMin);\n" + " b3Mat3x3 m;\n" + " m = b3QuatGetRotationMatrix(orn);\n" + " b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);\n" + " b3Float4 center = b3TransformPoint(localCenter,pos,orn);\n" + " \n" + " b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents,b3GetRow(abs_b,0)),\n" + " b3Dot3F4(localHalfExtents,b3GetRow(abs_b,1)),\n" + " b3Dot3F4(localHalfExtents,b3GetRow(abs_b,2)),\n" + " 0.f);\n" + " *aabbMinOut = center-extent;\n" + " *aabbMaxOut = center+extent;\n" + "}\n" + "/// conservative test for overlap between two aabbs\n" + "inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1,b3Float4ConstArg aabbMax1,\n" + " b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)\n" + "{\n" + " bool overlap = true;\n" + " overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;\n" + " overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;\n" + " overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;\n" + " return overlap;\n" + "}\n" + "#endif //B3_AABB_H\n" + "#ifndef B3_COLLIDABLE_H\n" + "#define B3_COLLIDABLE_H\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "enum b3ShapeTypes\n" + "{\n" + " SHAPE_HEIGHT_FIELD=1,\n" + " SHAPE_CONVEX_HULL=3,\n" + " SHAPE_PLANE=4,\n" + " SHAPE_CONCAVE_TRIMESH=5,\n" + " SHAPE_COMPOUND_OF_CONVEX_HULLS=6,\n" + " SHAPE_SPHERE=7,\n" + " MAX_NUM_SHAPE_TYPES,\n" + "};\n" + "typedef struct b3Collidable b3Collidable_t;\n" + "struct b3Collidable\n" + "{\n" + " union {\n" + " int m_numChildShapes;\n" + " int m_bvhIndex;\n" + " };\n" + " union\n" + " {\n" + " float m_radius;\n" + " int m_compoundBvhIndex;\n" + " };\n" + " int m_shapeType;\n" + " union\n" + " {\n" + " int m_shapeIndex;\n" + " float m_height;\n" + " };\n" + "};\n" + "typedef struct b3GpuChildShape b3GpuChildShape_t;\n" + "struct b3GpuChildShape\n" + "{\n" + " b3Float4 m_childPosition;\n" + " b3Quat m_childOrientation;\n" + " union\n" + " {\n" + " int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS\n" + " int m_capsuleAxis;\n" + " };\n" + " union \n" + " {\n" + " float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES\n" + " int m_numChildShapes;//used for compound shape\n" + " };\n" + " union \n" + " {\n" + " float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES\n" + " int m_collidableShapeIndex;\n" + " };\n" + " int m_shapeType;\n" + "};\n" + "struct b3CompoundOverlappingPair\n" + "{\n" + " int m_bodyIndexA;\n" + " int m_bodyIndexB;\n" + "// int m_pairType;\n" + " int m_childShapeIndexA;\n" + " int m_childShapeIndexB;\n" + "};\n" + "#endif //B3_COLLIDABLE_H\n" + "#ifndef B3_RIGIDBODY_DATA_H\n" + "#define B3_RIGIDBODY_DATA_H\n" + "#ifndef B3_FLOAT4_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_FLOAT4_H\n" + "#ifndef B3_QUAT_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif \n" + "#endif //B3_QUAT_H\n" + "#ifndef B3_MAT3x3_H\n" + "#ifdef __cplusplus\n" + "#else\n" + "#endif\n" + "#endif //B3_MAT3x3_H\n" + "typedef struct b3RigidBodyData b3RigidBodyData_t;\n" + "struct b3RigidBodyData\n" + "{\n" + " b3Float4 m_pos;\n" + " b3Quat m_quat;\n" + " b3Float4 m_linVel;\n" + " b3Float4 m_angVel;\n" + " int m_collidableIdx;\n" + " float m_invMass;\n" + " float m_restituitionCoeff;\n" + " float m_frictionCoeff;\n" + "};\n" + "typedef struct b3InertiaData b3InertiaData_t;\n" + "struct b3InertiaData\n" + "{\n" + " b3Mat3x3 m_invInertiaWorld;\n" + " b3Mat3x3 m_initInvInertia;\n" + "};\n" + "#endif //B3_RIGIDBODY_DATA_H\n" + " \n" + "void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)\n" + "{\n" + " __global const b3RigidBodyData_t* body = &bodies[bodyId];\n" + " b3Float4 position = body->m_pos;\n" + " b3Quat orientation = body->m_quat;\n" + " \n" + " int collidableIndex = body->m_collidableIdx;\n" + " int shapeIndex = collidables[collidableIndex].m_shapeIndex;\n" + " \n" + " if (shapeIndex>=0)\n" + " {\n" + " \n" + " b3Aabb_t localAabb = localShapeAABB[collidableIndex];\n" + " b3Aabb_t worldAabb;\n" + " \n" + " b3Float4 aabbAMinOut,aabbAMaxOut; \n" + " float margin = 0.f;\n" + " b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut);\n" + " \n" + " worldAabb.m_minVec =aabbAMinOut;\n" + " worldAabb.m_minIndices[3] = bodyId;\n" + " worldAabb.m_maxVec = aabbAMaxOut;\n" + " worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1;\n" + " worldAabbs[bodyId] = worldAabb;\n" + " }\n" + "}\n" + "#endif //B3_UPDATE_AABBS_H\n" + "__kernel void initializeGpuAabbsFull( const int numNodes, __global b3RigidBodyData_t* gBodies,__global b3Collidable_t* collidables, __global b3Aabb_t* plocalShapeAABB, __global b3Aabb_t* pAABB)\n" + "{\n" + " int nodeID = get_global_id(0);\n" + " if( nodeID < numNodes )\n" + " {\n" + " b3ComputeWorldAabb(nodeID, gBodies, collidables, plocalShapeAABB,pAABB);\n" + " }\n" + "}\n" + "__kernel void clearOverlappingPairsKernel( __global int4* pairs, int numPairs)\n" + "{\n" + " int pairId = get_global_id(0);\n" + " if( pairId< numPairs )\n" + " {\n" + " pairs[pairId].z = 0xffffffff;\n" + " }\n" + "}\n"; |