diff options
Diffstat (limited to 'thirdparty/bullet/src/Bullet3OpenCL/RigidBody')
39 files changed, 18289 insertions, 0 deletions
diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h new file mode 100644 index 0000000000..c7478f54a1 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h @@ -0,0 +1,18 @@ + +#ifndef B3_CONSTRAINT4_h +#define B3_CONSTRAINT4_h +#include "Bullet3Common/b3Vector3.h" + +#include "Bullet3Dynamics/shared/b3ContactConstraint4.h" + + +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]; } +}; + +#endif //B3_CONSTRAINT4_h + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp new file mode 100644 index 0000000000..af687b54e9 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp @@ -0,0 +1,137 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#include "b3GpuGenericConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include <new> +#include "Bullet3Common/b3Transform.h" + +void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyData* bodies) +{ + switch (m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + *info = 3; + break; + }; + default: + { + b3Assert(0); + } + }; +} + +void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) +{ + b3Transform trA; + trA.setIdentity(); + trA.setOrigin(bodies[constraint->m_rbA].m_pos); + trA.setRotation(bodies[constraint->m_rbA].m_quat); + + b3Transform trB; + trB.setIdentity(); + 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; + + 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 a1neg = -a1; + 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; + } + + b3Vector3 a2 = trB.getBasis()*constraint->getPivotInB(); + + { + // 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); + } + + + + // 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]); + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } +#if 0 + if(m_flags & B3_P2P_FLAGS_CFM) + { + for (j=0; j<3; j++) + { + info->cfm[j*info->rowskip] = m_cfm; + } + } +#endif + +#if 0 + b3Scalar impulseClamp = m_setting.m_impulseClamp;// + for (j=0; j<3; j++) + { + if (m_setting.m_impulseClamp > 0) + { + info->m_lowerLimit[j*info->rowskip] = -impulseClamp; + info->m_upperLimit[j*info->rowskip] = impulseClamp; + } + } + info->m_damping = m_setting.m_damping; +#endif + +} + +void b3GpuGenericConstraint::getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyData* bodies) +{ + switch (m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + getInfo2Point2Point(this,info,bodies); + break; + }; + default: + { + b3Assert(0); + } + }; +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h new file mode 100644 index 0000000000..14b3ba7fec --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h @@ -0,0 +1,132 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_GENERIC_CONSTRAINT_H +#define B3_GPU_GENERIC_CONSTRAINT_H + +#include "Bullet3Common/b3Quaternion.h" +struct b3RigidBodyData; +enum B3_CONSTRAINT_FLAGS +{ + 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_MAX_CONSTRAINT_TYPE +}; + + + +struct b3GpuConstraintInfo2 +{ + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + 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; + + // elements to jump from one row to the next in J's + int rowskip; + + // 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; + + // lo and hi limits for variables (set to -/+ infinity on entry). + 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; + // number of solver iterations + int m_numIterations; + + //damping of the velocity + b3Scalar m_damping; +}; + + +B3_ATTRIBUTE_ALIGNED16(struct) b3GpuGenericConstraint +{ + 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_uid; + int m_padding[2]; + + int getRigidBodyA() const + { + return m_rbA; + } + int getRigidBodyB() const + { + return m_rbB; + } + + const b3Vector3& getPivotInA() const + { + return m_pivotInA; + } + + const b3Vector3& getPivotInB() const + { + return m_pivotInB; + } + + int isEnabled() const + { + return m_flags & B3_CONSTRAINT_FLAG_ENABLED; + } + + float getBreakingImpulseThreshold() const + { + return m_breakingImpulseThreshold; + } + + ///internal method used by the constraint solver, don't use them directly + 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); + + +}; + +#endif //B3_GPU_GENERIC_CONSTRAINT_H
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp new file mode 100644 index 0000000000..179dfc4f26 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp @@ -0,0 +1,1382 @@ + +#include "b3GpuJacobiContactSolver.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3FillCL.h" //b3Int2 +class b3Vector3; +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "Bullet3OpenCL/RigidBody/kernels/solverUtils.h" +#include "Bullet3Common/b3Logging.h" +#include "b3GpuConstraint4.h" +#include "Bullet3Common/shared/b3Int2.h" +#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; + + + + 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_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); + + cl_int pErrNum; + const char* additionalMacros=""; + const char* solverUtilsSource = solverUtilsCL; + { + 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 ); + b3Assert(m_data->m_countBodiesKernel); + + 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 ); + b3Assert(m_data->m_clearVelocitiesKernel); + + 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 ); + 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_solveFrictionKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverUtilsSource, "SolveFrictionJacobiKernel", &pErrNum, solverUtilsProg,additionalMacros ); + b3Assert(m_data->m_solveFrictionKernel); + } + +} + + +b3GpuJacobiContactSolver::~b3GpuJacobiContactSolver() +{ + clReleaseKernel(m_data->m_solveContactKernel); + clReleaseKernel(m_data->m_solveFrictionKernel); + clReleaseKernel(m_data->m_countBodiesKernel); + clReleaseKernel(m_data->m_contactToConstraintSplitKernel); + clReleaseKernel(m_data->m_averageVelocitiesKernel); + clReleaseKernel(m_data->m_updateBodyVelocitiesKernel); + clReleaseKernel(m_data->m_clearVelocitiesKernel ); + + delete m_data->m_deltaLinearVelocities; + delete m_data->m_deltaAngularVelocities; + delete m_data->m_contactConstraints; + delete m_data->m_offsetSplitBodies; + delete m_data->m_contactConstraintOffsets; + delete m_data->m_bodyCount; + delete m_data->m_filler; + delete m_data->m_scan; + delete m_data; +} + + + +b3Vector3 make_float4(float v) +{ + return b3MakeVector3 (v,v,v); +} + +b3Vector4 make_float4(float x,float y, float z, float 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) +{ + + + for(int ic=0; ic<4; ic++) + { + // dont necessary because this makes change to 0 + 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 ); + + 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] ); + 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; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + + if (invMassA) + { + dLinVelA += linImp0; + dAngVelA += angImp0; + } + if (invMassB) + { + dLinVelB += linImp1; + dAngVelB += angImp1; + } + } + } +} + + + +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) +{ + float minRambdaDt = 0; + float maxRambdaDt = FLT_MAX; + + for(int ic=0; ic<4; ic++) + { + 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 ); + + 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 ); + 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; + + if (invMassA) + { + *dLinVelA += linImp0; + *dAngVelA += angImp0; + } + if (invMassB) + { + *dLinVelB += linImp1; + *dAngVelB += angImp1; + } + } +} + + +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; + + 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]); +#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] ); +#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]; + + { + 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; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + if (invMassA) + { + dLinVelA += linImp0; + dAngVelA += angImp0; + } + if (invMassB) + { + dLinVelB += linImp1; + dAngVelB += 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 ); + + if (invMassA) + dAngVelA -= (angNA*0.1f)*n; + if (invMassB) + 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) +{ + // 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)); + +} + + +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++) + { + 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++) + { + b3Vector3 r0 = src->m_worldPosB[ic] - posA; + b3Vector3 r1 = src->m_worldPosB[ic] - posB; + + if( ic >= src->m_worldNormalOnB[3] )//npoints + { + dstC->m_jacCoeffInv[ic] = 0.f; + continue; + } + + float relVelN; + { + b3Vector3 linear, angular0, angular1; + setLinearAndAngular(src->m_worldNormalOnB, r0, r1, linear, angular0, angular1); + + dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB ,countA,countB); + + relVelN = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB); + + float e = 0.f;//src->getRestituitionCoeff(); + if( relVelN*relVelN < 0.004f ) + { + e = 0.f; + } + + 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_appliedRambdaDt[ic] = 0.f; + } + } + + 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++) + center += src->m_worldPosB[i]; + center /= (float)src->m_worldNormalOnB[3]; + + b3Vector3 tangent[2]; + 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++) + { + 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); + dstC->m_fAppliedRambdaDt[i] = 0.f; + } + dstC->m_center = center; + } + + for(int i=0; i<4; i++) + { + if( i<src->m_worldNormalOnB[3] ) + { + dstC->m_worldPos[i] = src->m_worldPosB[i]; + } + else + { + dstC->m_worldPos[i] = make_float4(0.f); + } + } +} + + + +void ContactToConstraintKernel(b3Contact4* gContact, b3RigidBodyData* gBodies, b3InertiaData* gShapes, b3GpuConstraint4* gConstraintOut, int nContacts, +float dt, +float positionDrift, +float positionConstraintCoeff, int gIdx, b3AlignedObjectArray<unsigned int>& bodyCount +) +{ + //int gIdx = 0;//GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); + int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); + + b3Vector3 posA = gBodies[aIdx].m_pos; + 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; + + 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; + + 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 ); + + + + 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) +{ + B3_PROFILE("b3GpuJacobiContactSolver::solveGroup"); + + b3AlignedObjectArray<unsigned int> bodyCount; + bodyCount.resize(numBodies); + for (int i=0;i<numBodies;i++) + bodyCount[i] = 0; + + b3AlignedObjectArray<b3Int2> contactConstraintOffsets; + contactConstraintOffsets.resize(numManifolds); + + + 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); + + int bodyIndexA = manifoldPtr[i].getBodyA(); + int bodyIndexB = manifoldPtr[i].getBodyB(); + + if (!isFixedA) + { + contactConstraintOffsets[i].x = bodyCount[bodyIndexA]; + bodyCount[bodyIndexA]++; + } + if (!isFixedB) + { + 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]; + totalNumSplitBodies += numlastBody; + printf("totalNumSplitBodies = %d\n",totalNumSplitBodies); + + + + + + b3AlignedObjectArray<b3GpuConstraint4> contactConstraints; + contactConstraints.resize(numManifolds); + + 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); + } + 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++) + { + deltaLinearVelocities[i].setZero(); + deltaAngularVelocities[i].setZero(); + } + + + + for (int iter = 0;iter<maxIter;iter++) + { + 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; + + if (bodyA.m_invMass) + { + int bodyOffsetA = offsetSplitBodies[aIdx]; + int constraintOffsetA = contactConstraintOffsets[i].x; + int splitIndexA = bodyOffsetA+constraintOffsetA; + dlvAPtr = &deltaLinearVelocities[splitIndexA]; + davAPtr = &deltaAngularVelocities[splitIndexA]; + } + + if (bodyB.m_invMass) + { + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[i].y; + 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 ); + + + } + + } + + + //easy + 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); + b3Vector3 averageLinVel; + averageLinVel.setZero(); + b3Vector3 averageAngVel; + averageAngVel.setZero(); + for (int j=0;j<count;j++) + { + averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor; + averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor; + } + for (int j=0;j<count;j++) + { + deltaLinearVelocities[bodyOffset+j] = averageLinVel; + deltaAngularVelocities[bodyOffset+j] = averageAngVel; + } + } + } + } + for (int iter = 0;iter<maxIter;iter++) + { + //int i=0; + + //solve friction + + 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 sum = 0; + for(int j=0; j<4; j++) + { + sum +=contactConstraints[i].m_appliedRambdaDt[j]; + } + 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; + + if (bodyA.m_invMass) + { + int bodyOffsetA = offsetSplitBodies[aIdx]; + int constraintOffsetA = contactConstraintOffsets[i].x; + int splitIndexA = bodyOffsetA+constraintOffsetA; + dlvAPtr = &deltaLinearVelocities[splitIndexA]; + davAPtr = &deltaAngularVelocities[splitIndexA]; + } + + if (bodyB.m_invMass) + { + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[i].y; + int splitIndexB= bodyOffsetB+constraintOffsetB; + dlvBPtr =&deltaLinearVelocities[splitIndexB]; + davBPtr = &deltaAngularVelocities[splitIndexB]; + } + + for(int j=0; j<4; j++) + { + 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); + + } + + //easy + 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); + b3Vector3 averageLinVel; + averageLinVel.setZero(); + b3Vector3 averageAngVel; + averageAngVel.setZero(); + for (int j=0;j<count;j++) + { + averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor; + averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor; + } + for (int j=0;j<count;j++) + { + deltaLinearVelocities[bodyOffset+j] = averageLinVel; + deltaAngularVelocities[bodyOffset+j] = averageAngVel; + } + } + } + + + + } + + + //easy + for (int i=0;i<numBodies;i++) + { + if (bodies[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + if (count) + { + bodies[i].m_linVel += deltaLinearVelocities[bodyOffset]; + bodies[i].m_angVel += deltaAngularVelocities[bodyOffset]; + } + } + } +} + + + +void b3GpuJacobiContactSolver::solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config, int static0Index) +// +// +//void b3GpuJacobiContactSolver::solveGroup(b3OpenCLArray<b3RigidBodyData>* bodies,b3OpenCLArray<b3InertiaData>* inertias,b3OpenCLArray<b3Contact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo) +{ + b3JacobiSolverInfo solverInfo; + solverInfo.m_fixedBodyIndex = static0Index; + + + B3_PROFILE("b3GpuJacobiContactSolver::solveGroup"); + + //int numBodies = bodies->size(); + int numManifolds = numContacts;//manifoldPtr->size(); + + { + B3_PROFILE("resize"); + m_data->m_bodyCount->resize(numBodies); + } + + unsigned int val=0; + b3Int2 val2; + 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); + } + + { + B3_PROFILE("m_countBodiesKernel"); + 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; + { + 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); + } + + { + B3_PROFILE("m_data->m_contactConstraints->resize"); + //int numContacts = manifoldPtr->size(); + m_data->m_contactConstraints->resize(numContacts); + } + + { + B3_PROFILE("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(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + 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"); + 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++) + { + { + B3_PROFILE("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); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + + + { + B3_PROFILE("average velocities"); + 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()); + 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("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); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + + { + B3_PROFILE("average velocities"); + 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()); + 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 + +void b3GpuJacobiContactSolver::solveGroupMixed(b3OpenCLArray<b3RigidBodyData>* bodiesGPU,b3OpenCLArray<b3InertiaData>* inertiasGPU,b3OpenCLArray<b3Contact4>* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo) +{ + + b3AlignedObjectArray<b3RigidBodyData> bodiesCPU; + bodiesGPU->copyToHost(bodiesCPU); + b3AlignedObjectArray<b3InertiaData> inertiasCPU; + inertiasGPU->copyToHost(inertiasCPU); + b3AlignedObjectArray<b3Contact4> manifoldPtrCPU; + manifoldPtrGPU->copyToHost(manifoldPtrCPU); + + int numBodiesCPU = bodiesGPU->size(); + int numManifoldsCPU = manifoldPtrGPU->size(); + B3_PROFILE("b3GpuJacobiContactSolver::solveGroupMixed"); + + b3AlignedObjectArray<unsigned int> bodyCount; + bodyCount.resize(numBodiesCPU); + for (int i=0;i<numBodiesCPU;i++) + bodyCount[i] = 0; + + b3AlignedObjectArray<b3Int2> contactConstraintOffsets; + contactConstraintOffsets.resize(numManifoldsCPU); + + + for (int i=0;i<numManifoldsCPU;i++) + { + int pa = manifoldPtrCPU[i].m_bodyAPtrAndSignBit; + int pb = manifoldPtrCPU[i].m_bodyBPtrAndSignBit; + + bool isFixedA = (pa <0) || (pa == solverInfo.m_fixedBodyIndex); + bool isFixedB = (pb <0) || (pb == solverInfo.m_fixedBodyIndex); + + int bodyIndexA = manifoldPtrCPU[i].getBodyA(); + int bodyIndexB = manifoldPtrCPU[i].getBodyB(); + + if (!isFixedA) + { + contactConstraintOffsets[i].x = bodyCount[bodyIndexA]; + bodyCount[bodyIndexA]++; + } + if (!isFixedB) + { + contactConstraintOffsets[i].y = bodyCount[bodyIndexB]; + bodyCount[bodyIndexB]++; + } + } + + b3AlignedObjectArray<unsigned int> offsetSplitBodies; + offsetSplitBodies.resize(numBodiesCPU); + unsigned int totalNumSplitBodiesCPU; + m_data->m_scan->executeHost(bodyCount,offsetSplitBodies,numBodiesCPU,&totalNumSplitBodiesCPU); + int numlastBody = bodyCount[numBodiesCPU-1]; + totalNumSplitBodiesCPU += numlastBody; + + int numBodies = bodiesGPU->size(); + int numManifolds = manifoldPtrGPU->size(); + + m_data->m_bodyCount->resize(numBodies); + + unsigned int val=0; + b3Int2 val2; + 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); + } + + { + B3_PROFILE("m_countBodiesKernel"); + b3LauncherCL launcher(this->m_queue,m_data->m_countBodiesKernel); + launcher.setBuffer(manifoldPtrGPU->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; + 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); + + if (totalNumSplitBodies != totalNumSplitBodiesCPU) + { + printf("error in totalNumSplitBodies!\n"); + } + + int numContacts = manifoldPtrGPU->size(); + m_data->m_contactConstraints->resize(numContacts); + + + { + B3_PROFILE("contactToConstraintSplitKernel"); + b3LauncherCL launcher( m_queue, m_data->m_contactToConstraintSplitKernel); + launcher.setBuffer(manifoldPtrGPU->getBufferCL()); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(inertiasGPU->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(m_data->m_bodyCount->getBufferCL()); + launcher.setConst(numContacts); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.launch1D( numContacts, 64 ); + clFinish(m_queue); + } + + + + b3AlignedObjectArray<b3GpuConstraint4> contactConstraints; + contactConstraints.resize(numManifoldsCPU); + + for (int i=0;i<numManifoldsCPU;i++) + { + ContactToConstraintKernel(&manifoldPtrCPU[0],&bodiesCPU[0],&inertiasCPU[0],&contactConstraints[0],numManifoldsCPU, + solverInfo.m_deltaTime, + solverInfo.m_positionDrift, + solverInfo.m_positionConstraintCoeff, + i, bodyCount); + } + int maxIter = solverInfo.m_numIterations; + + + b3AlignedObjectArray<b3Vector3> deltaLinearVelocities; + b3AlignedObjectArray<b3Vector3> deltaAngularVelocities; + deltaLinearVelocities.resize(totalNumSplitBodiesCPU); + deltaAngularVelocities.resize(totalNumSplitBodiesCPU); + for (int i=0;i<totalNumSplitBodiesCPU;i++) + { + deltaLinearVelocities[i].setZero(); + deltaAngularVelocities[i].setZero(); + } + + m_data->m_deltaLinearVelocities->resize(totalNumSplitBodies); + m_data->m_deltaAngularVelocities->resize(totalNumSplitBodies); + + + + { + B3_PROFILE("m_clearVelocitiesKernel"); + b3LauncherCL launch(m_queue,m_data->m_clearVelocitiesKernel); + launch.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launch.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launch.setConst(totalNumSplitBodies); + launch.launch1D(totalNumSplitBodies); + } + + + ///!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + + m_data->m_contactConstraints->copyToHost(contactConstraints); + m_data->m_offsetSplitBodies->copyToHost(offsetSplitBodies); + m_data->m_contactConstraintOffsets->copyToHost(contactConstraintOffsets); + m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities); + m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities); + + for (int iter = 0;iter<maxIter;iter++) + { + + { + B3_PROFILE("m_solveContactKernel"); + b3LauncherCL launcher( m_queue, m_data->m_solveContactKernel ); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(inertiasGPU->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + + int i=0; + for( i=0; i<numManifoldsCPU; i++) + { + + float frictionCoeff = contactConstraints[i].getFrictionCoeff(); + int aIdx = (int)contactConstraints[i].m_bodyA; + int bIdx = (int)contactConstraints[i].m_bodyB; + b3RigidBodyData& bodyA = bodiesCPU[aIdx]; + b3RigidBodyData& bodyB = bodiesCPU[bIdx]; + + b3Vector3 zero(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; + dlvAPtr = &deltaLinearVelocities[splitIndexA]; + davAPtr = &deltaAngularVelocities[splitIndexA]; + } + + if (bodyB.m_invMass) + { + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[i].y; + 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, inertiasCPU[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertiasCPU[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr ); + + + } + } + + + { + B3_PROFILE("average velocities"); + b3LauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel); + launcher.setBuffer(bodiesGPU->getBufferCL()); + 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); + } + + //easy + for (int i=0;i<numBodiesCPU;i++) + { + if (bodiesCPU[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + float factor = 1.f/float(count); + b3Vector3 averageLinVel; + averageLinVel.setZero(); + b3Vector3 averageAngVel; + averageAngVel.setZero(); + for (int j=0;j<count;j++) + { + averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor; + averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor; + } + for (int j=0;j<count;j++) + { + deltaLinearVelocities[bodyOffset+j] = averageLinVel; + deltaAngularVelocities[bodyOffset+j] = averageAngVel; + } + } + } +// m_data->m_deltaAngularVelocities->copyFromHost(deltaAngularVelocities); + //m_data->m_deltaLinearVelocities->copyFromHost(deltaLinearVelocities); + m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities); + m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities); + +#if 0 + + { + B3_PROFILE("m_solveFrictionKernel"); + b3LauncherCL launcher( m_queue, m_data->m_solveFrictionKernel); + launcher.setBuffer(m_data->m_contactConstraints->getBufferCL()); + launcher.setBuffer(bodiesGPU->getBufferCL()); + launcher.setBuffer(inertiasGPU->getBufferCL()); + launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL()); + launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL()); + launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL()); + launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL()); + launcher.setConst(solverInfo.m_deltaTime); + launcher.setConst(solverInfo.m_positionDrift); + launcher.setConst(solverInfo.m_positionConstraintCoeff); + launcher.setConst(solverInfo.m_fixedBodyIndex); + launcher.setConst(numManifolds); + + launcher.launch1D(numManifolds); + clFinish(m_queue); + } + + //solve friction + + for(int i=0; i<numManifoldsCPU; i++) + { + 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++) + { + sum +=contactConstraints[i].m_appliedRambdaDt[j]; + } + float frictionCoeff = contactConstraints[i].getFrictionCoeff(); + int aIdx = (int)contactConstraints[i].m_bodyA; + int bIdx = (int)contactConstraints[i].m_bodyB; + b3RigidBodyData& bodyA = bodiesCPU[aIdx]; + b3RigidBodyData& bodyB = bodiesCPU[bIdx]; + + b3Vector3 zero(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; + dlvAPtr = &deltaLinearVelocities[splitIndexA]; + davAPtr = &deltaAngularVelocities[splitIndexA]; + } + + if (bodyB.m_invMass) + { + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[i].y; + int splitIndexB= bodyOffsetB+constraintOffsetB; + dlvBPtr =&deltaLinearVelocities[splitIndexB]; + davBPtr = &deltaAngularVelocities[splitIndexB]; + } + + for(int j=0; j<4; j++) + { + 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,inertiasCPU[aIdx].m_invInertiaWorld, + (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, inertiasCPU[bIdx].m_invInertiaWorld, + maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr); + + } + + { + B3_PROFILE("average velocities"); + b3LauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel); + launcher.setBuffer(bodiesGPU->getBufferCL()); + 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); + } + + //easy + for (int i=0;i<numBodiesCPU;i++) + { + if (bodiesCPU[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + float factor = 1.f/float(count); + b3Vector3 averageLinVel; + averageLinVel.setZero(); + b3Vector3 averageAngVel; + averageAngVel.setZero(); + for (int j=0;j<count;j++) + { + averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor; + averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor; + } + for (int j=0;j<count;j++) + { + deltaLinearVelocities[bodyOffset+j] = averageLinVel; + deltaAngularVelocities[bodyOffset+j] = averageAngVel; + } + } + } + +#endif + + } + + { + B3_PROFILE("update body velocities"); + b3LauncherCL launcher( m_queue, m_data->m_updateBodyVelocitiesKernel); + launcher.setBuffer(bodiesGPU->getBufferCL()); + 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); + } + + + //easy + for (int i=0;i<numBodiesCPU;i++) + { + if (bodiesCPU[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + if (count) + { + bodiesCPU[i].m_linVel += deltaLinearVelocities[bodyOffset]; + bodiesCPU[i].m_angVel += deltaAngularVelocities[bodyOffset]; + } + } + } + + +// bodiesGPU->copyFromHost(bodiesCPU); + + +} +#endif diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h new file mode 100644 index 0000000000..b418f29ec4 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h @@ -0,0 +1,62 @@ + +#ifndef B3_GPU_JACOBI_CONTACT_SOLVER_H +#define B3_GPU_JACOBI_CONTACT_SOLVER_H +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +//#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" + + +//struct b3InertiaData; +//b3InertiaData + +class b3TypedConstraint; + +struct b3JacobiSolverInfo +{ + int m_fixedBodyIndex; + + float m_deltaTime; + float m_positionDrift; + float m_positionConstraintCoeff; + int m_numIterations; + + b3JacobiSolverInfo() + :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; + cl_device_id m_device; + 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(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 + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp new file mode 100644 index 0000000000..698fa15f96 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp @@ -0,0 +1,1107 @@ +#include "b3GpuNarrowPhase.h" + + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" +#include <string.h> +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h" +#include "Bullet3Geometry/b3AabbUtil.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h" + +#include "b3GpuNarrowPhaseInternalData.h" +#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 = new b3GpuNarrowPhaseInternalData(); + m_data->m_currentContactBuffer = 0; + + 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_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_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_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_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_convexPolyhedra.reserve(config.m_maxConvexShapes); + + 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_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_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_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; + delete m_data->m_pBufContactOutCPU; + delete m_data->m_bodyBufferCPU; + delete m_data->m_inertiaBufferCPU; + 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; + delete m_data->m_localShapeAABBGPU; + delete m_data->m_bodyBufferGPU; + delete m_data->m_convexFacesGPU; + delete m_data->m_gpuChildShapes; + delete m_data->m_convexPolyhedraGPU; + delete m_data->m_uniqueEdgesGPU; + 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_worldVertsA1GPU; + delete m_data->m_worldVertsB2GPU; + + delete m_data->m_bvhInfoGPU; + + 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++) + { + delete m_data->m_meshInterfaces[i]; + } + m_data->m_meshInterfaces.clear(); + m_data->m_bvhData.clear(); + delete m_data->m_treeNodesGPU; + delete m_data->m_subTreesGPU; + + + delete m_data->m_convexData; + delete m_data; +} + + +int b3GpuNarrowPhase::allocateCollidable() +{ + int curSize = m_data->m_collidablesCPU.size(); + 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); + } + return -1; + +} + + + + + +int b3GpuNarrowPhase::registerSphereShape(float radius) +{ + int collidableIndex = allocateCollidable(); + 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) + { + b3SapAabb aabb; + 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_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_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU->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); + return faceOffset; +} + +int b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float planeConstant) +{ + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + + b3Collidable& col = getCollidableCpu(collidableIndex); + col.m_shapeType = SHAPE_PLANE; + col.m_shapeIndex = registerFace(planeNormal,planeConstant); + col.m_radius = planeConstant; + + 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); + clFinish(m_queue); + } + + return collidableIndex; +} + + +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); + + + 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_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); + + //convex data here + int i; + for ( i=0;i<convexPtr->m_uniqueEdges.size();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); + + + 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]); + + + 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_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++) + { + 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) +{ + b3AlignedObjectArray<b3Vector3> verts; + + 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])); + } + + b3ConvexUtility* utilPtr = new b3ConvexUtility(); + bool merge = true; + if (numVertices) + { + utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge); + } + + int collidableIndex = registerConvexHullShape(utilPtr); + delete utilPtr; + return collidableIndex; +} + +int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr) +{ + int collidableIndex = allocateCollidable(); + 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()); + utilPtr->m_localCenter = localCenter; + + col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col); + } + + 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++) + { + myAabbMin.setMin(utilPtr->m_vertices[i]); + myAabbMax.setMax(utilPtr->m_vertices[i]); + } + aabb.m_min[0] = myAabbMin[0]; + aabb.m_min[1] = myAabbMin[1]; + aabb.m_min[2] = myAabbMin[2]; + 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; + + m_data->m_localShapeAABBCPU->push_back(aabb); +// m_data->m_localShapeAABBGPU->push_back(aabb); + } + + return collidableIndex; + +} + +int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes) +{ + + int collidableIndex = allocateCollidable(); + if (collidableIndex<0) + return collidableIndex; + + b3Collidable& col = getCollidableCpu(collidableIndex); + col.m_shapeType = SHAPE_COMPOUND_OF_CONVEX_HULLS; + col.m_shapeIndex = m_data->m_cpuChildShapes.size(); + 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++) + { + 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); + + b3AlignedObjectArray<b3Aabb> childLocalAabbs; + childLocalAabbs.resize(childShapes->size()); + + //compute local AABB of the compound of all children + 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); + + 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); + myAabbMin.setMin(aMin); + myAabbMax.setMax(aMax); + childLocalAabbs[i].m_min[0] = aMin[0]; + childLocalAabbs[i].m_min[1] = aMin[1]; + childLocalAabbs[i].m_min[2] = aMin[2]; + childLocalAabbs[i].m_min[3] = 0; + childLocalAabbs[i].m_max[0] = aMax[0]; + childLocalAabbs[i].m_max[1] = aMax[1]; + 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_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_signedMaxIndices[3] = 0; + + m_data->m_localShapeAABBCPU->push_back(aabbLocalSpace); + + + b3QuantizedBvh* bvh = new b3QuantizedBvh; + bvh->setQuantizationValues(myAabbMin,myAabbMax); + QuantizedNodeArray& nodes = bvh->getLeafNodeArray(); + int numNodes = childShapes->size(); + + for (int i=0;i<numNodes;i++) + { + b3QuantizedBvhNode node; + 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); + int partId = 0; + node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; + nodes.push_back(node); + } + bvh->buildInternal(); + + int numSubTrees = bvh->getSubtreeInfoArray().size(); + + //void setQuantizationValues(const b3Vector3& bvhAabbMin,const b3Vector3& bvhAabbMax,b3Scalar quantizationMargin=b3Scalar(1.0)); + //QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } + ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized + //void buildInternal(); + + b3BvhInfo bvhInfo; + + bvhInfo.m_aabbMin = bvh->m_bvhAabbMin; + bvhInfo.m_aabbMax = bvh->m_bvhAabbMax; + bvhInfo.m_quantization = bvh->m_bvhQuantization; + bvhInfo.m_numNodes = numNodes; + bvhInfo.m_numSubTrees = numSubTrees; + bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size(); + bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size(); + + int numNewNodes = bvh->getQuantizedNodeArray().size(); + + 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++) + { + 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); + } + 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); + } + + } + } + + } + + 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.push_back(bvh->getSubtreeInfoArray()[i]); + } + int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); + + for (int i=0;i<numNewTreeNodes;i++) + { + m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]); + } + +// m_data->m_localShapeAABBGPU->push_back(aabbWS); + clFinish(m_queue); + return collidableIndex; + +} + + +int b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling1) +{ + + + b3Vector3 scaling=b3MakeVector3(scaling1[0],scaling1[1],scaling1[2]); + + int collidableIndex = allocateCollidable(); + 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_bvhIndex = m_data->m_bvhInfoCPU.size(); + + + b3SapAabb aabb; + b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f); + b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f); + + for (int i=0;i<vertices->size();i++) + { + b3Vector3 vtx(vertices->at(i)*scaling); + myAabbMin.setMin(vtx); + myAabbMax.setMax(vtx); + } + aabb.m_min[0] = myAabbMin[0]; + aabb.m_min[1] = myAabbMin[1]; + aabb.m_min[2] = myAabbMin[2]; + 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; + + m_data->m_localShapeAABBCPU->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(); + m_data->m_meshInterfaces.push_back(meshInterface); + b3IndexedMesh mesh; + mesh.m_numTriangles = indices->size()/3; + mesh.m_numVertices = vertices->size(); + 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); + + meshInterface->addIndexedMesh(mesh); + bvh->build(meshInterface, useQuantizedAabbCompression, (b3Vector3&)aabb.m_min, (b3Vector3&)aabb.m_max); + m_data->m_bvhData.push_back(bvh); + int numNodes = bvh->getQuantizedNodeArray().size(); + //b3OpenCLArray<b3QuantizedBvhNode>* treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context,this->m_queue,numNodes); + 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; + bvhInfo.m_numNodes = numNodes; + bvhInfo.m_numSubTrees = numSubTrees; + bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size(); + bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size(); + + 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.push_back(bvh->getSubtreeInfoArray()[i]); + } + int numNewTreeNodes = bvh->getQuantizedNodeArray().size(); + + 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) +{ + + + 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); + 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++) + { + 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 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); + 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++) + { + 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++) + { + 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() +{ + return (cl_mem)m_data->m_bodyBufferGPU->getBufferCL(); +} + +const struct b3RigidBodyData* b3GpuNarrowPhase::getBodiesCpu() const +{ + return &m_data->m_bodyBufferCPU->at(0); +}; + + + + +int b3GpuNarrowPhase::getNumBodiesGpu() const +{ + return m_data->m_bodyBufferGPU->size(); +} + +cl_mem b3GpuNarrowPhase::getBodyInertiasGpu() +{ + return (cl_mem)m_data->m_inertiaBufferGPU->getBufferCL(); +} + +int b3GpuNarrowPhase::getNumBodyInertiasGpu() const +{ + return m_data->m_inertiaBufferGPU->size(); +} + + +b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex) +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + +const b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex) const +{ + return m_data->m_collidablesCPU[collidableIndex]; +} + +cl_mem b3GpuNarrowPhase::getCollidablesGpu() +{ + return m_data->m_collidablesGPU->getBufferCL(); +} + +const struct b3Collidable* b3GpuNarrowPhase::getCollidablesCpu() const +{ + if (m_data->m_collidablesCPU.size()) + return &m_data->m_collidablesCPU[0]; + return 0; +} + +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() +{ + return m_data->m_localShapeAABBGPU->getBufferCL(); +} +int b3GpuNarrowPhase::getNumCollidablesGpu() const +{ + return m_data->m_collidablesGPU->size(); +} + + + + + +int b3GpuNarrowPhase::getNumContactsGpu() const +{ + return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); +} +cl_mem b3GpuNarrowPhase::getContactsGpu() +{ + return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->getBufferCL(); +} + +const b3Contact4* b3GpuNarrowPhase::getContactsCPU() const +{ + m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->copyToHost(*m_data->m_pBufContactOutCPU); + return &m_data->m_pBufContactOutCPU->at(0); +} + +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; + + //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); + + + + + 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); + + 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_config.m_maxContactCapacity, + m_data->m_config.m_compoundPairCapacity, + *m_data->m_convexPolyhedraGPU, + *m_data->m_convexVerticesGPU, + *m_data->m_uniqueEdgesGPU, + *m_data->m_convexFacesGPU, + *m_data->m_convexIndicesGPU, + *m_data->m_collidablesGPU, + *m_data->m_gpuChildShapes, + clAabbArrayWorldSpace, + clAabbArrayLocalSpace, + *m_data->m_worldVertsB1GPU, + *m_data->m_clippingFacesOutGPU, + *m_data->m_worldNormalsAGPU, + *m_data->m_worldVertsA1GPU, + *m_data->m_worldVertsB2GPU, + m_data->m_bvhData, + m_data->m_treeNodesGPU, + m_data->m_subTreesGPU, + m_data->m_bvhInfoGPU, + numObjects, + maxTriConvexPairCapacity, + *m_data->m_triangleConvexPairs, + numTriConvexPairsOut + ); + + /*b3AlignedObjectArray<b3Int4> broadphasePairsCPU; + broadphasePairsGPU.copyToHost(broadphasePairsCPU); + printf("checking pairs\n"); + */ +} + +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) +{ + 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); + return -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_collidableIdx = collidableIndex; + if (collidableIndex>=0) + { +// body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType; + } else + { + // 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; + + if (writeToGpu) + { + 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 (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 + { + + 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 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)); + + b3Vector3 invLocalInertia; + 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]); + + 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); + + + + return m_data->m_numAcceleratedRigidBodies++; +} + +int b3GpuNarrowPhase::getNumRigidBodies() const +{ + return m_data->m_numAcceleratedRigidBodies; +} + +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); + m_data->m_uniqueEdgesGPU->copyFromHost(m_data->m_uniqueEdges); + m_data->m_convexVerticesGPU->copyFromHost(m_data->m_convexVertices); + m_data->m_convexIndicesGPU->copyFromHost(m_data->m_convexIndices); + m_data->m_bvhInfoGPU->copyFromHost(m_data->m_bvhInfoCPU); + 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); + } + if (m_data->m_collidablesCPU.size()) + { + m_data->m_collidablesGPU->copyFromHost(m_data->m_collidablesCPU); + } + + +} + + +void b3GpuNarrowPhase::reset() +{ + m_data->m_numAcceleratedShapes = 0; + m_data->m_numAcceleratedRigidBodies = 0; + this->m_static0Index = -1; + m_data->m_uniqueEdges.resize(0); + m_data->m_convexVertices.resize(0); + m_data->m_convexPolyhedra.resize(0); + m_data->m_convexIndices.resize(0); + m_data->m_cpuChildShapes.resize(0); + m_data->m_convexFaces.resize(0); + m_data->m_collidablesCPU.resize(0); + m_data->m_localShapeAABBCPU->resize(0); + m_data->m_bvhData.resize(0); + m_data->m_treeNodesCPU.resize(0); + m_data->m_subTreesCPU.resize(0); + m_data->m_bvhInfoCPU.resize(0); + +} + + +void b3GpuNarrowPhase::readbackAllBodiesToCpu() +{ + m_data->m_bodyBufferGPU->copyToHostPointer(&m_data->m_bodyBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies); +} + +void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation , int bodyIndex) +{ + 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]); + } + else + { + b3Warning("setObjectVelocityCpu out of range.\n"); + } +} +void b3GpuNarrowPhase::setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex) +{ + 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 + { + b3Warning("setObjectVelocityCpu out of range.\n"); + } +} + +bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const +{ + 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 + + orientation[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.x; + orientation[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.y; + orientation[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.z; + orientation[3] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.w; + return true; + } + + b3Warning("getObjectTransformFromCpu out of range.\n"); + return false; +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h new file mode 100644 index 0000000000..05ff3fd09e --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h @@ -0,0 +1,109 @@ +#ifndef B3_GPU_NARROWPHASE_H +#define B3_GPU_NARROWPHASE_H + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" + +class b3GpuNarrowPhase +{ +protected: + + struct b3GpuNarrowPhaseInternalData* m_data; + int m_acceleratedCompanionShapeIndex; + int m_planeBodyIndex; + int m_static0Index; + + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + + int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col); + 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 registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes); + int registerFace(const b3Vector3& faceNormal, float faceConstant); + + 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); + + void writeAllBodiesToGpu(); + void reset(); + void readbackAllBodiesToCpu(); + bool getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const; + + 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(); + const struct b3RigidBodyData* getBodiesCpu() const; + //struct b3RigidBodyData* getBodiesCpu(); + + int getNumBodiesGpu() const; + + cl_mem getBodyInertiasGpu(); + int getNumBodyInertiasGpu() const; + + cl_mem getCollidablesGpu(); + const struct b3Collidable* getCollidablesCpu() const; + int getNumCollidablesGpu() const; + + const struct b3SapAabb* getLocalSpaceAabbsCpu() const; + + const struct b3Contact4* getContactsCPU() const; + + cl_mem getContactsGpu(); + int getNumContactsGpu() const; + + cl_mem getAabbLocalSpaceBufferGpu(); + + int getNumRigidBodies() const; + + int allocateCollidable(); + + int getStatic0Index() const + { + return m_static0Index; + } + b3Collidable& getCollidableCpu(int collidableIndex); + const b3Collidable& getCollidableCpu(int collidableIndex) const; + + const b3GpuNarrowPhaseInternalData* getInternalData() const + { + return m_data; + } + + b3GpuNarrowPhaseInternalData* getInternalData() + { + return m_data; + } + + const struct b3SapAabb& getLocalSpaceAabb(int collidableIndex) const; +}; + +#endif //B3_GPU_NARROWPHASE_H + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h new file mode 100644 index 0000000000..8a7f1ea859 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h @@ -0,0 +1,95 @@ + +#ifndef B3_GPU_NARROWPHASE_INTERNAL_DATA_H +#define B3_GPU_NARROWPHASE_INTERNAL_DATA_H + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Vector3.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" + +#include "Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h" +#include "Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h" +#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; + + b3AlignedObjectArray<b3GpuChildShape> m_cpuChildShapes; + b3OpenCLArray<b3GpuChildShape>* m_gpuChildShapes; + + b3AlignedObjectArray<b3GpuFace> m_convexFaces; + b3OpenCLArray<b3GpuFace>* m_convexFacesGPU; + + struct GpuSatCollision* m_gpuSatCollision; + + + b3OpenCLArray<b3Int4>* m_triangleConvexPairs; + + + b3OpenCLArray<b3Contact4>* m_pBufContactBuffersGPU[2]; + int m_currentContactBuffer; + b3AlignedObjectArray<b3Contact4>* m_pBufContactOutCPU; + + + b3AlignedObjectArray<b3RigidBodyData>* m_bodyBufferCPU; + b3OpenCLArray<b3RigidBodyData>* m_bodyBufferGPU; + + b3AlignedObjectArray<b3InertiaData>* m_inertiaBufferCPU; + b3OpenCLArray<b3InertiaData>* m_inertiaBufferGPU; + + int m_numAcceleratedShapes; + int m_numAcceleratedRigidBodies; + + b3AlignedObjectArray<b3Collidable> m_collidablesCPU; + b3OpenCLArray<b3Collidable>* m_collidablesGPU; + + b3OpenCLArray<b3SapAabb>* m_localShapeAABBGPU; + b3AlignedObjectArray<b3SapAabb>* m_localShapeAABBCPU; + + b3AlignedObjectArray<class b3OptimizedBvh*> m_bvhData; + b3AlignedObjectArray<class b3TriangleIndexVertexArray*> m_meshInterfaces; + + b3AlignedObjectArray<b3QuantizedBvhNode> m_treeNodesCPU; + b3AlignedObjectArray<b3BvhSubtreeInfo> m_subTreesCPU; + + b3AlignedObjectArray<b3BvhInfo> m_bvhInfoCPU; + b3OpenCLArray<b3BvhInfo>* m_bvhInfoGPU; + + b3OpenCLArray<b3QuantizedBvhNode>* m_treeNodesGPU; + b3OpenCLArray<b3BvhSubtreeInfo>* m_subTreesGPU; + + + b3Config m_config; + +}; + +#endif //B3_GPU_NARROWPHASE_INTERNAL_DATA_H diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp new file mode 100644 index 0000000000..0d3d50c548 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp @@ -0,0 +1,1158 @@ + +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + + +bool useGpuInitSolverBodies = true; +bool useGpuInfo1 = true; +bool useGpuInfo2= true; +bool useGpuSolveJointConstraintRows=true; +bool useGpuWriteBackVelocities = true; +bool gpuBreakConstraints = true; + +#include "b3GpuPgsConstraintSolver.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" +#include <new> +#include "Bullet3Common/b3AlignedObjectArray.h" +#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/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; + + cl_kernel m_solveJointConstraintRowsKernels; + cl_kernel m_initSolverBodiesKernel; + cl_kernel m_getInfo1Kernel; + cl_kernel m_initBatchConstraintsKernel; + cl_kernel m_getInfo2Kernel; + cl_kernel m_writeBackVelocitiesKernel; + cl_kernel m_breakViolatedConstraintsKernel; + + b3OpenCLArray<unsigned int>* m_gpuConstraintRowOffsets; + + 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<b3RigidBodyData> m_cpuBodies; + b3AlignedObjectArray<b3InertiaData> m_cpuInertias; + + + b3AlignedObjectArray<b3GpuGenericConstraint> m_cpuConstraints; + + b3AlignedObjectArray<int> m_batchSizes; + + +}; + + +/* +static b3Transform getWorldTransform(b3RigidBodyData* rb) +{ + b3Transform newTrans; + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); + return newTrans; +} + +static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia) +{ + return inertia->m_invInertiaWorld; +} + +*/ + +static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb) +{ + return rb->m_linVel; +} + +static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb) +{ + return rb->m_angVel; +} + +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) +{ + m_usePgs = usePgs; + m_gpuData = new b3GpuPgsJacobiSolverInternalData(); + m_gpuData->m_context = ctx; + m_gpuData->m_device = device; + m_gpuData->m_queue = 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_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,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); + + + + + clReleaseProgram(prog); + } + + +} + +b3GpuPgsConstraintSolver::~b3GpuPgsConstraintSolver () +{ + clReleaseKernel(m_gpuData->m_solveJointConstraintRowsKernels); + clReleaseKernel(m_gpuData->m_initSolverBodiesKernel); + clReleaseKernel(m_gpuData->m_getInfo1Kernel); + clReleaseKernel(m_gpuData->m_initBatchConstraintsKernel); + clReleaseKernel(m_gpuData->m_getInfo2Kernel); + clReleaseKernel(m_gpuData->m_writeBackVelocitiesKernel); + clReleaseKernel(m_gpuData->m_breakViolatedConstraintsKernel); + + delete m_gpuData->m_prefixScan; + delete m_gpuData->m_gpuConstraintRowOffsets; + delete m_gpuData->m_gpuSolverBodies; + delete m_gpuData->m_gpuBatchConstraints; + delete m_gpuData->m_gpuConstraintRows; + delete m_gpuData->m_gpuConstraintInfo1; + + delete m_gpuData; +} + +struct b3BatchConstraint +{ + int m_bodyAPtrAndSignBit; + int m_bodyBPtrAndSignBit; + int m_originalConstraintIndex; + int m_batchId; +}; + +static b3AlignedObjectArray<b3BatchConstraint> batchConstraints; + + +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) +{ + B3_PROFILE("GPU solveGroupCacheFriendlySetup"); + batchConstraints.resize(numConstraints); + m_gpuData->m_gpuBatchConstraints->resize(numConstraints); + m_staticIdx = -1; + m_maxOverrideNumSolverIterations = 0; + + + /* m_gpuData->m_gpuBodies->resize(numBodies); + m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies); + + b3OpenCLArray<b3InertiaData> gpuInertias(m_gpuData->m_context,m_gpuData->m_queue); + gpuInertias.resize(numBodies); + gpuInertias.copyFromHostPointer(inertias,numBodies); + */ + + 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"); + launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL()); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setConst(numBodies); + launcher.launch1D(numBodies); + clFinish(m_gpuData->m_queue); + + // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + } else + { + gpuBodies->copyToHost(m_gpuData->m_cpuBodies); + for (int i=0;i<numBodies;i++) + { + + b3RigidBodyData& body = m_gpuData->m_cpuBodies[i]; + b3GpuSolverBody& solverBody = m_tmpSolverBodyPool[i]; + initSolverBody(i,&solverBody,&body); + solverBody.m_originalBodyIndex = i; + } + m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); + } + } + +// 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"); + launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL()); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + clFinish(m_gpuData->m_queue); + } + + 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; + + { + B3_PROFILE("init batch constraints"); + 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()); + launcher.setBuffer(gpuConstraints->getBufferCL()); + launcher.setBuffer(gpuBodies->getBufferCL()); + launcher.setConst(numConstraints); + launcher.launch1D(numConstraints); + clFinish(m_gpuData->m_queue); + } + //assume the batching happens on CPU, so copy the data + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + } + } + else + { + totalNumRows = 0; + gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); + //calculate the total number of contraint rows + for (int i=0;i<numConstraints;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 + { + 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); + + + + } + } + else + { + + gpuInertias->copyToHost(m_gpuData->m_cpuInertias); + + ///setup the b3SolverConstraints + + for (int i=0;i<numConstraints;i++) + { + const int& info1 = m_tmpConstraintSizesPool[i]; + + if (info1) + { + int constraintIndex = batchConstraints[i].m_originalConstraintIndex; + int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[constraintIndex]; + + b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[constraintRowOffset]; + b3GpuGenericConstraint& constraint = m_gpuData->m_cpuConstraints[i]; + + b3RigidBodyData& rbA = m_gpuData->m_cpuBodies[ constraint.getRigidBodyA()]; + //b3RigidBody& rbA = constraint.getRigidBodyA(); + // 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); + + b3GpuSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + b3GpuSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + if (rbA.m_invMass) + { + batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA; + } else + { + if (!solverBodyIdA) + m_staticIdx = 0; + batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA; + } + + if (rbB.m_invMass) + { + batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; + } 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) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; + + + int 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); + 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_friction = 0.f; + currentConstraintRow[j].m_frictionIndex = 0; + currentConstraintRow[j].m_jacDiagABInv = 0.f; + currentConstraintRow[j].m_lowerLimit = 0.f; + currentConstraintRow[j].m_upperLimit = 0.f; + + 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_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; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + 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); + + + b3GpuConstraintInfo2 info2; + 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 + ///the size of b3GpuSolverConstraint needs be a multiple of b3Scalar + 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; + info2.cfm = ¤tConstraintRow->m_cfm; + 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]); + + ///finalize the constraint setup + for ( j=0;j<info1;j++) + { + b3GpuSolverConstraint& solverConstraint = currentConstraintRow[j]; + + 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()) + { + solverConstraint.m_lowerLimit = -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold(); + } + + // 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; + } + + b3Matrix3x3& invInertiaWorldB= m_gpuData->m_cpuInertias[constraint.getRigidBodyB()].m_invInertiaWorld; + { + + const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; + 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; + + b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal); + 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; + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + b3Scalar rel_vel; + 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; + + 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; + 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) + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + else + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + + m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); + + + + }//end useGpuInfo2 + + + } + +#ifdef B3_SUPPORT_CONTACT_CONSTRAINTS + { + int i; + + for (i=0;i<numManifolds;i++) + { + b3Contact4& manifold = manifoldPtr[i]; + convertContact(bodies,inertias,&manifold,infoGlobal); + } + } +#endif //B3_SUPPORT_CONTACT_CONSTRAINTS + } + +// 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) +{ + body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor; + body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor); +} + + +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); + + 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); + c->m_appliedImpulse = c->m_lowerLimit; + } + else if (sum > c->m_upperLimit) + { + deltaImpulse = c->m_upperLimit-b3Scalar(c->m_appliedImpulse); + c->m_appliedImpulse = c->m_upperLimit; + } + else + { + 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); + +} + + + +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); + + b3Assert(rb); +// 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_linearVelocity = getLinearVelocity(rb); + solverBody->m_angularVelocity = getAngularVelocity(rb); +} + + +void b3GpuPgsConstraintSolver::averageVelocities() +{ +} + + +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; + { + + 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; + int numBodies = m_tmpSolverBodyPool.size(); + sortConstraintByBatch3( &batchConstraints[0], numConstraints, simdWidth , m_staticIdx, numBodies); + + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + + } + } else + { + /*b3AlignedObjectArray<b3BatchConstraint> cpuCheckBatches; + m_gpuData->m_gpuBatchConstraints->copyToHost(cpuCheckBatches); + b3Assert(cpuCheckBatches.size()==batchConstraints.size()); + printf(".\n"); + */ + //>copyFromHost(batchConstraints); + } + int maxIterations = infoGlobal.m_numIterations; + + bool useBatching = true; + + if (useBatching ) + { + + if (!useGpuSolveJointConstraintRows) + { + B3_PROFILE("copy to host"); + m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints); + m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool); + 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++) + { + + int batchOffset = 0; + int constraintOffset=0; + int numBatches = m_gpuData->m_batchSizes.size(); + 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, + __global b3SolverConstraint* rows, + __global unsigned int* numConstraintRowsInfo1, + __global unsigned int* rowOffsets, + __global b3GpuGenericConstraint* constraints, + int batchOffset, + int numConstraintsInBatch*/ + + + 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.setConst(batchOffset); + launcher.setConst(numConstraintsInBatch); + + launcher.launch1D(numConstraintsInBatch); + + + } else//useGpu + { + + + + for (int b=0;b<numConstraintsInBatch;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); + b3GpuGenericConstraint* constraint = &m_gpuData->m_cpuConstraints[c.m_originalConstraintIndex]; + 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++) + { + // + 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); + } + } + } + }//useGpu + batchOffset+=numConstraintsInBatch; + constraintOffset+=numConstraintsInBatch; + } + }//for (int iteration... + + if (!useGpuSolveJointConstraintRows) + { + { + B3_PROFILE("copy from host"); + m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool); + m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints); + m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool); + } + + //B3_PROFILE("copy to host"); + //m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool); + } + //int sz = sizeof(b3GpuSolverBody); + //printf("cpu sizeof(b3GpuSolverBody)=%d\n",sz); + + + + + + } else + { + 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); + } + + if (!m_usePgs) + { + averageVelocities(); + } + } + } + + } + 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) +{ + //int sz = sizeof(b3BatchConstraint); + + B3_PROFILE("sortConstraintByBatch3"); + + static int maxSwaps = 0; + int numSwaps = 0; + + curUsed.resize(2*simdWidth); + + static int maxNumConstraints = 0; + if (maxNumConstraints<numConstraints) + { + maxNumConstraints = numConstraints; + //printf("maxNumConstraints = %d\n",maxNumConstraints ); + } + + int numUsedArray = numBodies/32+1; + bodyUsed.resize(numUsedArray); + + 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++) + cs[i].m_batchId = -1; +#endif + + int numValidConstraints = 0; +// int unprocessedConstraintIndex = 0; + + int batchIdx = 0; + + + { + B3_PROFILE("cpu batch innerloop"); + + while( numValidConstraints < numConstraints) + { + numIter++; + int nCurrentBatch = 0; + // clear flag + for(int i=0; i<curBodyUsed; i++) + bodyUsed[curUsed[i]/32] = 0; + + curBodyUsed = 0; + + for(int i=numValidConstraints; i<numConstraints; i++) + { + int idx = i; + 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; + int aUnavailable = 0; + int bUnavailable = 0; + if (!aIsStatic) + { + 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 (!aIsStatic) + { + bodyUsed[ bodyA/32 ] |= (1<<(bodyA&31)); + curUsed[curBodyUsed++]=bodyA; + } + if (!bIsStatic) + { + bodyUsed[ bodyB/32 ] |= (1<<(bodyB&31)); + curUsed[curBodyUsed++]=bodyB; + } + + cs[idx].m_batchId = batchIdx; + + if (i!=numValidConstraints) + { + b3Swap(cs[i],cs[numValidConstraints]); + numSwaps++; + } + + numValidConstraints++; + { + nCurrentBatch++; + if( nCurrentBatch == simdWidth ) + { + nCurrentBatch = 0; + for(int i=0; i<curBodyUsed; i++) + bodyUsed[curUsed[i]/32] = 0; + curBodyUsed = 0; + } + } + } + } + m_gpuData->m_batchSizes.push_back(nCurrentBatch); + batchIdx ++; + } + } + +#if defined(_DEBUG) + // debugPrintf( "nBatches: %d\n", batchIdx ); + for(int i=0; i<numConstraints; i++) + { + b3Assert( cs[i].m_batchId != -1 ); + } +#endif + + 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) +{ + + B3_PROFILE("solveJoints"); + //you need to provide at least some bodies + + solveGroupCacheFriendlySetup( gpuBodies, gpuInertias,numBodies,gpuConstraints, numConstraints,infoGlobal); + + solveGroupCacheFriendlyIterations(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) +{ + 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_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS; + 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); + +} + +//b3AlignedObjectArray<b3RigidBodyData> testBodies; + + +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; + + + { + if (gpuBreakConstraints) + { + B3_PROFILE("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 + { + gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); + m_gpuData->m_gpuBatchConstraints->copyToHost(m_gpuData->m_cpuBatchConstraints); + m_gpuData->m_gpuConstraintRows->copyToHost(m_gpuData->m_cpuConstraintRows); + gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints); + 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++) + { + 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++) + { + 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); + if (b3Fabs(m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold) + { + + m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED; + } + } + } + } + + + gpuConstraints->copyFromHost(m_gpuData->m_cpuConstraints); + } + } + + { + if (useGpuWriteBackVelocities) + { + B3_PROFILE("GPU write back velocities and transforms"); + + 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_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++) + { + int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex; + //printf("bodyIndex=%d\n",bodyIndex); + b3Assert(i==bodyIndex); + + b3RigidBodyData* body = &m_gpuData->m_cpuBodies[bodyIndex]; + if (body->m_invMass) + { + if (infoGlobal.m_splitImpulse) + m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + else + m_tmpSolverBodyPool[i].writebackVelocity(); + + if (m_usePgs) + { + body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity; + body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity; + } else + { + b3Assert(0); + } + /* + if (infoGlobal.m_splitImpulse) + { + body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin(); + b3Quaternion orn; + orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation(); + body->m_quat = orn; + } + */ + } + }//for + + gpuBodies->copyFromHost(m_gpuData->m_cpuBodies); + + } + } + + clFinish(m_gpuData->m_queue); + + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); + + m_tmpSolverBodyPool.resizeNoInitialize(0); + return 0.f; +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h new file mode 100644 index 0000000000..ec0e3f73d6 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h @@ -0,0 +1,78 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_PGS_CONSTRAINT_SOLVER_H +#define B3_GPU_PGS_CONSTRAINT_SOLVER_H + +struct b3Contact4; +struct b3ContactPoint; + + +class b3Dispatcher; + +#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h" +#include "Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h" +#include "b3GpuSolverBody.h" +#include "b3GpuSolverConstraint.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +struct b3RigidBodyData; +struct b3InertiaData; + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "b3GpuGenericConstraint.h" + +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; + + b3AlignedObjectArray<unsigned int> m_tmpConstraintSizesPool; + + + bool m_usePgs; + void averageVelocities(); + + int m_maxOverrideNumSolverIterations; + + int m_numSplitImpulseRecoveries; + +// 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); + + + 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(); +}; + +#endif //B3_GPU_PGS_CONSTRAINT_SOLVER_H diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp new file mode 100644 index 0000000000..f0b0abd5e0 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp @@ -0,0 +1,1708 @@ + +bool gUseLargeBatches = false; +bool gCpuBatchContacts = false; +bool gCpuSolveConstraint = false; +bool gCpuRadixSort=false; +bool gCpuSetSortData = false; +bool gCpuSortContactsDeterminism = false; +bool gUseCpuCopyConstraints = false; +bool gUseScanHost = false; +bool gReorderContactsOnCpu = false; + +bool optionalSortContactsDeterminism = true; + + +#include "b3GpuPgsContactSolver.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" + +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" +#include <string.h> +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#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" +#define B3_SOLVER_FRICTION_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl" +#define B3_BATCHING_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl" +#define B3_BATCHING_NEW_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl" + +#include "kernels/solverSetup.h" +#include "kernels/solverSetup2.h" +#include "kernels/solveContact.h" +#include "kernels/solveFriction.h" +#include "kernels/batchingKernels.h" +#include "kernels/batchingKernelsNew.h" + + + + + +struct b3GpuBatchingPgsSolverInternalData +{ + cl_context m_context; + cl_device_id m_device; + cl_command_queue m_queue; + int m_pairCapacity; + int m_nIterations; + + b3OpenCLArray<b3GpuConstraint4>* m_contactCGPU; + b3OpenCLArray<unsigned int>* m_numConstraints; + b3OpenCLArray<unsigned int>* m_offsets; + + b3Solver* m_solverGPU; + + cl_kernel m_batchingKernel; + cl_kernel m_batchingKernelNew; + cl_kernel m_solveContactKernel; + cl_kernel m_solveSingleContactKernel; + cl_kernel m_solveSingleFrictionKernel; + cl_kernel m_solveFrictionKernel; + cl_kernel m_contactToConstraintKernel; + cl_kernel m_setSortDataKernel; + cl_kernel m_reorderContactKernel; + cl_kernel m_copyConstraintKernel; + + 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; + + b3OpenCLArray<b3SortData>* m_sortDataBuffer; + b3OpenCLArray<b3Contact4>* m_contactBuffer; + + b3OpenCLArray<b3RigidBodyData>* m_bodyBufferGPU; + b3OpenCLArray<b3InertiaData>* m_inertiaBufferGPU; + b3OpenCLArray<b3Contact4>* m_pBufContactOutGPU; + + 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; + +}; + + + +b3GpuPgsContactSolver::b3GpuPgsContactSolver(cl_context ctx,cl_device_id device, cl_command_queue q,int pairCapacity) +{ + 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_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); + + 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_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_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; + 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); + b3Assert(solveContactProg); + + 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); + + + b3Assert(solverSetup2Prog); + + + 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 ); + b3Assert(m_data->m_solveFrictionKernel); + + 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 ); + b3Assert(m_data->m_solveSingleContactKernel); + + 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 ); + b3Assert(m_data->m_contactToConstraintKernel); + + 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 ); + b3Assert(m_data->m_setDeterminismSortDataBodyAKernel); + + 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 ); + b3Assert(m_data->m_setDeterminismSortDataChildShapeAKernel); + + 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 ); + b3Assert(m_data->m_reorderContactKernel); + + + 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); + b3Assert(batchingProg); + + 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); + b3Assert(batchingNewProg); + + m_data->m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg,additionalMacros ); + b3Assert(m_data->m_batchingKernelNew); + } + + + + + + + +} + +b3GpuPgsContactSolver::~b3GpuPgsContactSolver() +{ + delete m_data->m_batchSizesGpu; + delete m_data->m_bodyBufferGPU; + delete m_data->m_inertiaBufferGPU; + delete m_data->m_pBufContactOutGPU; + 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; + delete m_data->m_sortDataBuffer; + delete m_data->m_contactBuffer; + + delete m_data->m_sort32; + delete m_data->m_scan; + delete m_data->m_search; + delete m_data->m_solverGPU; + + clReleaseKernel(m_data->m_batchingKernel); + 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_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) {} + + float m_positionDrift; + float m_positionConstraintCoeff; + float m_dt; + bool m_enableParallelSolve; + float m_batchCellSize; + 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) +{ + B3_PROFILE("solveContactConstraintBatchSizes"); + int numBatches = batchSizes->size()/B3_MAX_NUM_BATCHES; + for(int iter=0; iter<numIterations; iter++) + { + + for (int cellId=0;cellId<numBatches;cellId++) + { + int offset = 0; + for (int ii=0;ii<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() ); + launcher.setConst(cellId); + launcher.setConst(offset); + launcher.setConst(numInBatch); + launcher.launch1D(numInBatch); + offset+=numInBatch; + } + } + } + } + + + for(int iter=0; iter<numIterations; iter++) + { + for (int cellId=0;cellId<numBatches;cellId++) + { + int offset = 0; + for (int ii=0;ii<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() ); + launcher.setConst(cellId); + launcher.setConst(offset); + launcher.setConst(numInBatch); + launcher.launch1D(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) +{ + + //sort the contacts + + + b3Int4 cdata = b3MakeInt4( n, 0, 0, 0 ); + { + + const int nn = B3_SOLVER_N_CELLS; + + cdata.x = 0; + cdata.y = maxNumBatches;//250; + + + int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; +#ifdef DEBUG_ME + 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 ib=0; ib<B3_SOLVER_N_BATCHES; ib++) + { +#ifdef DEBUG_ME + 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" ); +#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() ) +#ifdef DEBUG_ME + , b3BufferInfoCL(&gpuDebugInfo) +#endif + }; + + + + 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 ); + 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 ); + + +#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); + } + +#endif + + +#ifdef DEBUG_ME + clFinish(m_queue); + gpuDebugInfo.read(debugInfo,numWorkItems); + clFinish(m_queue); + for (int i=0;i<numWorkItems;i++) + { + if (debugInfo[i].m_valInt2>0) + { + printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2); + } + + if (debugInfo[i].m_valInt3>0) + { + printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3); + } + } +#endif //DEBUG_ME + + + } + } + + clFinish(m_data->m_queue); + + + } + + cdata.x = 1; + bool applyFriction=true; + if (applyFriction) + { + B3_PROFILE("m_batchSolveKernel iterations2"); + for(int iter=0; iter<numIterations; iter++) + { + 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() ) +#ifdef 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()); + //launcher.setConst( cdata.x ); + 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 ); + } + } + clFinish(m_data->m_queue); + + } +#ifdef DEBUG_ME + delete[] debugInfo; +#endif //DEBUG_ME + } + + +} + + + + + + + + + + + +static bool sortfnc(const b3SortData& a,const b3SortData& b) +{ + 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) + ); +} + + + + + + + + + + + +#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 + +}; + + +#endif + + +void SetSortDataCPU(b3Contact4* gContact, b3RigidBodyData* gBodies, b3SortData* gSortDataOut, int nContacts,float scale,const b3Int4& nSplit,int staticIdx) +{ + for (int gIdx=0;gIdx<nContacts;gIdx++) + { + if( gIdx < nContacts ) + { + int aPtrAndSignBit = gContact[gIdx].m_bodyAPtrAndSignBit; + int bPtrAndSignBit = gContact[gIdx].m_bodyBPtrAndSignBit; + + int aIdx = abs(aPtrAndSignBit ); + int bIdx = abs(bPtrAndSignBit); + + bool aStatic = (aPtrAndSignBit<0) ||(aPtrAndSignBit==staticIdx); + + #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; + if (aStatic) + aa = bb; + if (bStatic) + bb = aa; + + int gridIndex = aa + bb*4; + int newIndex = gridTable4x4[gridIndex]; + #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 newIndex = gridTable8x8[gridIndex]; + #endif//USE_4x4_GRID + #endif//USE_SPATIAL_BATCHING + + + gSortDataOut[gIdx].x = newIndex; + gSortDataOut[gIdx].y = gIdx; + } + else + { + gSortDataOut[gIdx].x = 0xffffffff; + } + } +} + + + + + + +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); + + if (optionalSortContactsDeterminism) + { + if (!gCpuSortContactsDeterminism) + { + B3_PROFILE("GPU Sort contact constraints (determinism)"); + + 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); + + { + 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 ); + } + m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); + { + 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 ); + } + m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); + { + 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 ); + } + + m_data->m_solverGPU->m_sort32->execute(*m_data->m_contactKeyValues); + + { + 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 ); + } + + 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"); + 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 + { + B3_PROFILE("CPU Sort contact constraints (determinism)"); + b3AlignedObjectArray<b3Contact4> cpuConstraints; + m_data->m_pBufContactOutGPU->copyToHost(cpuConstraints); + bool sort = true; + if (sort) + { + cpuConstraints.quickSort(b3ContactCmp); + + for (int i=0;i<cpuConstraints.size();i++) + { + cpuConstraints[i].m_batchIdx = i; + } + } + m_data->m_pBufContactOutGPU->copyFromHost(cpuConstraints); + if (m_debugOutput==100) + { + 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); + } + } + + 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; + + + 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); + + + + { + 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 ) + { + + + 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 + B3_PROFILE("GPU set cell idx"); + struct CB + { + int m_nContacts; + int m_staticIdx; + float m_scale; + b3Int4 m_nSplit; + }; + + 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_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 ); + launcher.setConst(cdata.m_nSplit); + launcher.setConst(cdata.m_staticIdx); + + + launcher.launch1D( sortSize, 64 ); + } else + { + m_data->m_solverGPU->m_sortDataBuffer->resize(nContacts); + b3AlignedObjectArray<b3SortData> sortDataCPU; + m_data->m_solverGPU->m_sortDataBuffer->copyToHost(sortDataCPU); + + b3AlignedObjectArray<b3Contact4> contactCPU; + m_data->m_pBufContactOutGPU->copyToHost(contactCPU); + b3AlignedObjectArray<b3RigidBodyData> bodiesCPU; + bodyBuf->copyToHost(bodiesCPU); + 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); + + + m_data->m_solverGPU->m_sortDataBuffer->copyFromHost(sortDataCPU); + } + + + + if (!gCpuRadixSort) + { // 3. sort by cell idx + B3_PROFILE("gpuRadixSort"); + //int n = B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT; + //int sortBit = 32; + //if( n <= 0xffff ) sortBit = 16; + //if( n <= 0xff ) sortBit = 8; + //adl::RadixSort<adl::TYPE_CL>::execute( data->m_sort, *data->m_sortDataBuffer, sortSize ); + //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 + { + b3OpenCLArray<b3SortData>& keyValuesInOut = *(m_data->m_solverGPU->m_sortDataBuffer); + b3AlignedObjectArray<b3SortData> hostValues; + keyValuesInOut.copyToHost(hostValues); + hostValues.quickSort(sortfnc); + keyValuesInOut.copyFromHost(hostValues); + } + + + if (gUseScanHost) + { + // 4. find entries + B3_PROFILE("cpuBoundSearch"); + b3AlignedObjectArray<unsigned int> countsHost; + countsNative->copyToHost(countsHost); + + 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); + + 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 ); + + //unsigned int sum; + //m_data->m_solverGPU->m_scan->execute(*countsNative,*offsetsNative, B3_SOLVER_N_CELLS);//,&sum ); + b3AlignedObjectArray<unsigned int> offsetsHost; + offsetsHost.resize(offsetsNative->size()); + + + m_data->m_solverGPU->m_scan->executeHost(countsHost,offsetsHost, B3_SOLVER_N_CELLS);//,&sum ); + offsetsNative->copyFromHost(offsetsHost); + + //printf("sum = %d\n",sum); + } 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 ); + } + + + + + if (nContacts) + { // 5. sort constraints by cellIdx + if (gReorderContactsOnCpu) + { + B3_PROFILE("cpu m_reorderContactKernel"); + b3AlignedObjectArray<b3SortData> sortDataHost; + m_data->m_solverGPU->m_sortDataBuffer->copyToHost(sortDataHost); + b3AlignedObjectArray<b3Contact4> inContacts; + b3AlignedObjectArray<b3Contact4> outContacts; + m_data->m_pBufContactOutGPU->copyToHost(inContacts); + outContacts.resize(inContacts.size()); + for (int i=0;i<nContacts;i++) + { + int srcIdx = sortDataHost[i].y; + outContacts[i] = inContacts[srcIdx]; + } + m_data->m_solverGPU->m_contactBuffer2->copyFromHost(outContacts); + + /* "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" + */ + } 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()) }; + + 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); + + // { + // b3AlignedObjectArray<unsigned int> histogram; + // m_data->m_solverGPU->m_numConstraints->copyToHost(histogram); + // printf(",,,\n"); + // } + + + if (nContacts) + { + + if (gUseCpuCopyConstraints) + { + 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() + } + + } 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 ); + //we use the clFinish for proper benchmark/profile + clFinish(m_data->m_queue); + } + } + + +// 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 ); + clFinish(m_data->m_queue); + } else + { + B3_PROFILE("cpu batchContacts"); + static b3AlignedObjectArray<b3Contact4> cpuContacts; + b3OpenCLArray<b3Contact4>* contactsIn = m_data->m_solverGPU->m_contactBuffer2; + { + B3_PROFILE("copyToHost"); + contactsIn->copyToHost(cpuContacts); + } + b3OpenCLArray<unsigned int>* countsNative = m_data->m_solverGPU->m_numConstraints; + b3OpenCLArray<unsigned int>* offsetsNative = m_data->m_solverGPU->m_offsets; + + b3AlignedObjectArray<unsigned int> nNativeHost; + b3AlignedObjectArray<unsigned int> offsetsNativeHost; + + { + B3_PROFILE("countsNative/offsetsNative copyToHost"); + countsNative->copyToHost(nNativeHost); + offsetsNative->copyToHost(offsetsNativeHost); + } + + + 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); + static int globalMaxBatch = 0; + if (maxNumBatches>globalMaxBatch ) + { + globalMaxBatch = maxNumBatches; + b3Printf("maxNumBatches = %d\n",maxNumBatches); + } + + } else + { + 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++) + { + int n = (nNativeHost)[i]; + int offset = (offsetsNativeHost)[i]; + 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); + static int globalMaxBatch = 0; + if (maxNumBatches>globalMaxBatch ) + { + globalMaxBatch = maxNumBatches; + b3Printf("maxNumBatches = %d\n",maxNumBatches); + } + //we use the clFinish for proper benchmark/profile + + } + } + //clFinish(m_data->m_queue); + } + { + B3_PROFILE("m_contactBuffer->copyFromHost"); + m_data->m_solverGPU->m_contactBuffer2->copyFromHost((b3AlignedObjectArray<b3Contact4>&)cpuContacts); + } + + } + + } + + + + + + } + + + } + + + //printf("maxNumBatches = %d\n", maxNumBatches); + + if (gUseLargeBatches) + { + if (nContacts) + { + B3_PROFILE("cpu batchContacts"); + static b3AlignedObjectArray<b3Contact4> cpuContacts; +// 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; + + + +// 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); + static int globalMaxBatch = 0; + if (maxNumBatches>globalMaxBatch ) + { + 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 ); + clFinish(m_data->m_queue); + } + + + if (1) + { + int numIter = 4; + + m_data->m_solverGPU->m_nIterations = numIter;//10 + if (!gCpuSolveConstraint) + { + B3_PROFILE("GPU solveContactConstraint"); + + /*m_data->m_solverGPU->solveContactConstraint( + m_data->m_bodyBufferGPU, + m_data->m_inertiaBufferGPU, + m_data->m_contactCGPU,0, + nContactOut , + maxNumBatches); + */ + + //m_data->m_batchSizesGpu->copyFromHost(m_data->m_batchSizes); + + if (gUseLargeBatches) + { + 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_inertiaBufferGPU, + 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); + } + + + } + + +#if 0 + if (0) + { + B3_PROFILE("read body velocities back to CPU"); + //read body updated linear/angular velocities back to CPU + m_data->m_bodyBufferGPU->read( + m_data->m_bodyBufferCPU->m_ptr,numOfConvexRBodies); + 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 ) +{ +} + + + + + + + + + + + +b3AlignedObjectArray<unsigned int> idxBuffer; +b3AlignedObjectArray<b3SortData> sortData; +b3AlignedObjectArray<b3Contact4> old; + + +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]; +#if defined(_DEBUG) + for(int i=0; i<n; i++) + cs[i].getBatchIdx() = -1; +#endif + for(int i=0; i<n; i++) + idxSrc[i] = i; + nIdxSrc = n; + + int batchIdx = 0; + + { + B3_PROFILE("cpu batch innerloop"); + 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++) + { + int idx = idxSrc[i]; + + + 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 + { + if (!aIsStatic) + flg[ aIdx/32 ] |= (1<<(aIdx&31)); + if (!bIsStatic) + flg[ bIdx/32 ] |= (1<<(bIdx&31)); + + cs[idx].getBatchIdx() = batchIdx; + sortData[idx].m_key = batchIdx; + sortData[idx].m_value = idx; + + { + nCurrentBatch++; + if( nCurrentBatch == simdWidth ) + { + nCurrentBatch = 0; + for(int i=0; i<N_FLG/32; i++) flg[i] = 0; + } + } + } + else + { + idxDst[nIdxDst++] = idx; + } + } + b3Swap( idxSrc, idxDst ); + b3Swap( nIdxSrc, nIdxDst ); + batchIdx ++; + } + } + { + B3_PROFILE("quickSort"); + sortData.quickSort(sortfnc); + } + + + { + B3_PROFILE("reorder"); + // reorder + + 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 ); + } +#endif + return batchIdx; +} + + +b3AlignedObjectArray<int> bodyUsed2; + +inline int b3GpuPgsContactSolver::sortConstraintByBatch2( b3Contact4* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies) +{ + + B3_PROFILE("sortConstraintByBatch2"); + + + + bodyUsed2.resize(2*simdWidth); + + 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++) + cs[i].getBatchIdx() = -1; +#endif + for(int i=0; i<numConstraints; i++) + idxSrc[i] = i; + + int numValidConstraints = 0; +// int unprocessedConstraintIndex = 0; + + int batchIdx = 0; + + + { + B3_PROFILE("cpu batch innerloop"); + + while( numValidConstraints < numConstraints) + { + numIter++; + int nCurrentBatch = 0; + // clear flag + for(int i=0; i<curBodyUsed; i++) + bodyUsed2[i] = 0; + curBodyUsed = 0; + + for(int i=numValidConstraints; i<numConstraints; i++) + { + int idx = idxSrc[i]; + 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; + int aUnavailable = 0; + int bUnavailable = 0; + if (!aIsStatic) + { + for (int j=0;j<curBodyUsed;j++) + { + if (bodyA == bodyUsed2[j]) + { + aUnavailable=1; + break; + } + } + } + if (!aUnavailable) + if (!bIsStatic) + { + for (int j=0;j<curBodyUsed;j++) + { + if (bodyB == bodyUsed2[j]) + { + bUnavailable=1; + break; + } + } + } + + if( aUnavailable==0 && bUnavailable==0 ) // ok + { + if (!aIsStatic) + { + bodyUsed2[curBodyUsed++] = bodyA; + } + if (!bIsStatic) + { + bodyUsed2[curBodyUsed++] = bodyB; + } + + cs[idx].getBatchIdx() = batchIdx; + m_data->m_sortData[idx].m_key = batchIdx; + m_data->m_sortData[idx].m_value = idx; + + if (i!=numValidConstraints) + { + b3Swap(idxSrc[i], idxSrc[numValidConstraints]); + } + + numValidConstraints++; + { + nCurrentBatch++; + if( nCurrentBatch == simdWidth ) + { + nCurrentBatch = 0; + for(int i=0; i<curBodyUsed; i++) + bodyUsed2[i] = 0; + + + curBodyUsed = 0; + } + } + } + } + + batchIdx ++; + } + } + { + B3_PROFILE("quickSort"); + //m_data->m_sortData.quickSort(sortfnc); + } + + { + B3_PROFILE("reorder"); + // reorder + + 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 ); + } +#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) +{ + + B3_PROFILE("sortConstraintByBatch3"); + + static int maxSwaps = 0; + int numSwaps = 0; + + curUsed.resize(2*simdWidth); + + static int maxNumConstraints = 0; + if (maxNumConstraints<numConstraints) + { + maxNumConstraints = numConstraints; + //printf("maxNumConstraints = %d\n",maxNumConstraints ); + } + + int numUsedArray = numBodies/32+1; + bodyUsed.resize(numUsedArray); + + 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++) + cs[i].getBatchIdx() = -1; +#endif + + int numValidConstraints = 0; +// int unprocessedConstraintIndex = 0; + + int batchIdx = 0; + + + { + B3_PROFILE("cpu batch innerloop"); + + while( numValidConstraints < numConstraints) + { + numIter++; + int nCurrentBatch = 0; + batchSizes[batchIdx] = 0; + + // clear flag + for(int i=0; i<curBodyUsed; i++) + bodyUsed[curUsed[i]/32] = 0; + + curBodyUsed = 0; + + for(int i=numValidConstraints; i<numConstraints; i++) + { + int idx = i; + 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; + int aUnavailable = 0; + int bUnavailable = 0; + if (!aIsStatic) + { + 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 (!aIsStatic) + { + bodyUsed[ bodyA/32 ] |= (1<<(bodyA&31)); + curUsed[curBodyUsed++]=bodyA; + } + if (!bIsStatic) + { + bodyUsed[ bodyB/32 ] |= (1<<(bodyB&31)); + curUsed[curBodyUsed++]=bodyB; + } + + cs[idx].getBatchIdx() = batchIdx; + + if (i!=numValidConstraints) + { + b3Swap(cs[i],cs[numValidConstraints]); + numSwaps++; + } + + numValidConstraints++; + { + nCurrentBatch++; + if( nCurrentBatch == simdWidth ) + { + batchSizes[batchIdx] += simdWidth; + nCurrentBatch = 0; + for(int i=0; i<curBodyUsed; i++) + bodyUsed[curUsed[i]/32] = 0; + curBodyUsed = 0; + } + } + } + } + + if (batchIdx>=B3_MAX_NUM_BATCHES) + { + b3Error("batchIdx>=B3_MAX_NUM_BATCHES"); + b3Assert(0); + break; + } + + batchSizes[batchIdx] += nCurrentBatch; + + batchIdx ++; + + } + } + +#if defined(_DEBUG) + // debugPrintf( "nBatches: %d\n", batchIdx ); + for(int i=0; i<numConstraints; i++) + { + b3Assert( cs[i].getBatchIdx() != -1 ); + } +#endif + + batchSizes[batchIdx] =0; + + if (maxSwaps<numSwaps) + { + maxSwaps = numSwaps; + //printf("maxSwaps = %d\n", maxSwaps); + } + + return batchIdx; +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h new file mode 100644 index 0000000000..98e2a5b8c4 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h @@ -0,0 +1,43 @@ + +#ifndef B3_GPU_BATCHING_PGS_SOLVER_H +#define B3_GPU_BATCHING_PGS_SOLVER_H + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "b3GpuConstraint4.h" + +class b3GpuPgsContactSolver +{ +protected: + + int m_debugOutput; + + struct b3GpuBatchingPgsSolverInternalData* m_data; + + 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); + + + + 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); + +public: + + 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 + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp new file mode 100644 index 0000000000..783e443060 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -0,0 +1,708 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#include "b3GpuRigidBodyPipeline.h" +#include "b3GpuRigidBodyPipelineInternalData.h" +#include "kernels/integrateKernel.h" +#include "kernels/updateAabbsKernel.h" + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "b3GpuNarrowPhase.h" +#include "Bullet3Geometry/b3AabbUtil.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h" +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" + +//#define TEST_OTHER_GPU_SOLVER + +#define B3_RIGIDBODY_INTEGRATE_PATH "src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl" +#define B3_RIGIDBODY_UPDATEAABB_PATH "src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl" + +bool useBullet2CpuSolver = true; + +//choice of contact solver +bool gUseJacobi = false; +bool gUseDbvt = false; +bool gDumpContactStats = false; +bool gCalcWorldSpaceAabbOnCpu = false; +bool gUseCalculateOverlappingPairsHost = false; +bool gIntegrateOnCpu = false; +bool gClearPairsOnGpu = true; + +#define TEST_OTHER_GPU_SOLVER 1 +#ifdef TEST_OTHER_GPU_SOLVER +#include "b3GpuJacobiContactSolver.h" +#endif //TEST_OTHER_GPU_SOLVER + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h" + +#include "b3GpuPgsContactSolver.h" +#include "b3Solver.h" + +#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) +{ + m_data = new b3GpuRigidBodyPipelineInternalData; + 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_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_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); + + 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); + 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); + + + 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; + delete m_data->m_solver; + delete m_data->m_allAabbsGPU; + delete m_data->m_gpuConstraints; + delete m_data->m_overlappingPairsGPU; + +#ifdef TEST_OTHER_GPU_SOLVER + delete m_data->m_solver3; +#endif //TEST_OTHER_GPU_SOLVER + + delete m_data->m_solver2; + + + delete m_data; +} + +void b3GpuRigidBodyPipeline::reset() +{ + m_data->m_gpuConstraints->resize(0); + m_data->m_cpuConstraints.resize(0); + m_data->m_allAabbsGPU->resize(0); + m_data->m_allAabbsCPU.resize(0); +} + +void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint) +{ + m_data->m_joints.push_back(constraint); +} + +void b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint) +{ + m_data->m_joints.remove(constraint); +} + + + +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++) + { + 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.pop_back(); + + break; + } + } + + if (m_data->m_cpuConstraints.size()) + { + m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints); + } else + { + m_data->m_gpuConstraints->resize(0); + } + +} +int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold) +{ + m_data->m_gpuSolver->recomputeBatches(); + b3GpuGenericConstraint c; + c.m_uid = m_data->m_constraintUid; + m_data->m_constraintUid++; + 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_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) +{ + m_data->m_gpuSolver->recomputeBatches(); + b3GpuGenericConstraint c; + c.m_uid = m_data->m_constraintUid; + m_data->m_constraintUid++; + 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_breakingImpulseThreshold = breakingThreshold; + c.m_constraintType = B3_GPU_FIXED_CONSTRAINT_TYPE; + + m_data->m_cpuConstraints.push_back(c); + return c.m_uid; +} + + +void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) +{ + + //update worldspace AABBs from local AABB/worldtransform + { + B3_PROFILE("setupGpuAabbs"); + setupGpuAabbsFull(); + } + + 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++) + { + 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); + } + } + + { + B3_PROFILE("calculateOverlappingPairs"); + m_data->m_broadphaseDbvt->calculateOverlappingPairs(); + } + numPairs = m_data->m_broadphaseDbvt->getOverlappingPairCache()->getNumOverlappingPairs(); + + } else + { + if (gUseCalculateOverlappingPairsHost) + { + m_data->m_broadphaseSap->calculateOverlappingPairsHost(m_data->m_config.m_maxBroadphasePairs); + } else + { + m_data->m_broadphaseSap->calculateOverlappingPairs(m_data->m_config.m_maxBroadphasePairs); + } + numPairs = m_data->m_broadphaseSap->getNumOverlap(); + } + } + + //compute contact points +// 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; + 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 + { + pairs = m_data->m_broadphaseSap->getOverlappingPairBuffer(); + aabbsWS = m_data->m_broadphaseSap->getAabbBufferWS(); + } + + m_data->m_overlappingPairsGPU->resize(numPairs); + + //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); + + if (gClearPairsOnGpu) + { + + + //b3AlignedObjectArray<b3BroadphasePair> hostPairs;//just for debugging + //gpuPairs.copyToHost(hostPairs); + + b3LauncherCL launcher(m_data->m_queue,m_data->m_clearOverlappingPairsKernel,"clearOverlappingPairsKernel"); + launcher.setBuffer(pairs); + launcher.setConst(numPairs); + launcher.launch1D(numPairs); + + + //gpuPairs.copyToHost(hostPairs); + + + } else + { + b3AlignedObjectArray<b3BroadphasePair> hostPairs; + gpuPairs.copyToHost(hostPairs); + + for (int i=0;i<hostPairs.size();i++) + { + hostPairs[i].z = 0xffffffff; + } + + gpuPairs.copyFromHost(hostPairs); + } + } + + m_data->m_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies); + numContacts = m_data->m_narrowphase->getNumContactsGpu(); + + if (gUseDbvt) + { + ///store the cached information (contact locations in the 'z' component) + B3_PROFILE("m_overlappingPairsGPU->copyToHost"); + m_data->m_overlappingPairsGPU->copyToHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray()); + } + if (gDumpContactStats && numContacts) + { + m_data->m_narrowphase->getContactsGpu(); + + printf("numContacts = %d\n", numContacts); + + int totalPoints = 0; + const b3Contact4* contacts = m_data->m_narrowphase->getContactsCPU(); + + for (int i=0;i<numContacts;i++) + { + totalPoints += contacts->getNPoints(); + } + 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()); + + int numJoints = m_data->m_joints.size() ? m_data->m_joints.size() : m_data->m_cpuConstraints.size(); + if (useBullet2CpuSolver && numJoints) + { + + // b3AlignedObjectArray<b3Contact4> hostContacts; + //gpuContacts.copyToHost(hostContacts); + { + bool useGpu = m_data->m_joints.size()==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 + { + 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); + gpuBodies.copyFromHost(hostBodies); + } + } + } + + 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); + gpuInertias.copyToHost(hostInertias); + gpuContacts.copyToHost(hostContacts); + } + + { + b3JacobiSolverInfo solverInfo; + m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),solverInfo); + + + } + { + B3_PROFILE("copyFromHost"); + gpuBodies.copyFromHost(hostBodies); + } + } 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); + } + } else + { + b3AlignedObjectArray<b3RigidBodyData> hostBodies; + gpuBodies.copyToHost(hostBodies); + b3AlignedObjectArray<b3InertiaData> hostInertias; + gpuInertias.copyToHost(hostInertias); + b3AlignedObjectArray<b3Contact4> hostContacts; + gpuContacts.copyToHost(hostContacts); + { + //m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]); + } + gpuBodies.copyFromHost(hostBodies); + } + + } 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_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, + (b3OpenCLArray<Constraint4>*) &gpuContacts, + 0,numContacts,256); + */ + } + } + + integrate(deltaTime); + +} + + +void b3GpuRigidBodyPipeline::integrate(float timeStep) +{ + //integrate + int numBodies = m_data->m_narrowphase->getNumRigidBodies(); + float angularDamp = 0.99f; + + if (gIntegrateOnCpu) + { + if(numBodies) + { + 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++) + { + integrateSingleTransform( bodies,nodeID, timeStep, angularDamp, m_data->m_gravity); + } + npData->m_bodyBufferGPU->copyFromHost(*npData->m_bodyBufferCPU); + } + } else + { + 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); + launcher.setConst(m_data->m_gravity); + launcher.launch1D(numBodies); + } +} + + + + +void b3GpuRigidBodyPipeline::setupGpuAabbsFull() +{ + cl_int ciErrNum=0; + + int numBodies = m_data->m_narrowphase->getNumRigidBodies(); + if (!numBodies) + return; + + if (gCalcWorldSpaceAabbOnCpu) + { + + if (numBodies) + { + if (gUseDbvt) + { + m_data->m_allAabbsCPU.resize(numBodies); + m_data->m_narrowphase->readbackAllBodiesToCpu(); + 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]); + } + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + } else + { + m_data->m_broadphaseSap->getAllAabbsCPU().resize(numBodies); + m_data->m_narrowphase->readbackAllBodiesToCpu(); + 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]); + } + m_data->m_broadphaseSap->getAllAabbsGPU().copyFromHost(m_data->m_broadphaseSap->getAllAabbsCPU()); + //m_data->m_broadphaseSap->writeAabbsToGpu(); + } + } + } 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"); + launcher.setConst(numBodies); + cl_mem bodies = m_data->m_narrowphase->getBodiesGpu(); + launcher.setBuffer(bodies); + cl_mem collidables = m_data->m_narrowphase->getCollidablesGpu(); + launcher.setBuffer(collidables); + cl_mem localAabbs = m_data->m_narrowphase->getAabbLocalSpaceBufferGpu(); + launcher.setBuffer(localAabbs); + + cl_mem worldAabbs =0; + if (gUseDbvt) + { + worldAabbs = m_data->m_allAabbsGPU->getBufferCL(); + } else + { + worldAabbs = m_data->m_broadphaseSap->getAabbBufferWS(); + } + launcher.setBuffer(worldAabbs); + launcher.launch1D(numBodies); + + oclCHECKERROR(ciErrNum, CL_SUCCESS); + } + + /* + b3AlignedObjectArray<b3SapAabb> aabbs; + m_data->m_broadphaseSap->m_allAabbsGPU.copyToHost(aabbs); + + printf("numAabbs = %d\n", aabbs.size()); + + for (int i=0;i<aabbs.size();i++) + { + printf("aabb[%d].m_min=%f,%f,%f,%d\n",i,aabbs[i].m_minVec[0],aabbs[i].m_minVec[1],aabbs[i].m_minVec[2],aabbs[i].m_minIndices[3]); + printf("aabb[%d].m_max=%f,%f,%f,%d\n",i,aabbs[i].m_maxVec[0],aabbs[i].m_maxVec[1],aabbs[i].m_maxVec[2],aabbs[i].m_signedMaxIndices[3]); + + }; + */ + + + + + +} + + + +cl_mem b3GpuRigidBodyPipeline::getBodyBuffer() +{ + return m_data->m_narrowphase->getBodiesGpu(); +} + +int b3GpuRigidBodyPipeline::getNumBodies() const +{ + return m_data->m_narrowphase->getNumRigidBodies(); +} + +void b3GpuRigidBodyPipeline::setGravity(const float* grav) +{ + m_data->m_gravity.setValue(grav[0],grav[1],grav[2]); +} + +void b3GpuRigidBodyPipeline::copyConstraintsToHost() +{ + m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints); +} + +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) +{ + + b3Vector3 aabbMin=b3MakeVector3(0,0,0),aabbMax=b3MakeVector3(0,0,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]); + + 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 + { + 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); + + if (bodyIndex>=0) + { + if (gUseDbvt) + { + m_data->m_broadphaseDbvt->createProxy(aabbMin,aabbMax,bodyIndex,0,1,1); + b3SapAabb aabb; + for (int i=0;i<3;i++) + { + aabb.m_min[i] = aabbMin[i]; + aabb.m_max[i] = aabbMax[i]; + aabb.m_minIndices[3] = bodyIndex; + } + m_data->m_allAabbsCPU.push_back(aabb); + if (writeInstanceToGpu) + { + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + } + } else + { + if (mass) + { + 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); + } + } + } + + /* + if (mass>0.f) + m_numDynamicPhysicsInstances++; + + m_numPhysicsInstances++; + */ + + return bodyIndex; +} + +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); +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h new file mode 100644 index 0000000000..b4eac6841a --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h @@ -0,0 +1,74 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_RIGIDBODY_PIPELINE_H +#define B3_GPU_RIGIDBODY_PIPELINE_H + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h" + +class b3GpuRigidBodyPipeline +{ +protected: + 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); + virtual ~b3GpuRigidBodyPipeline(); + + void stepSimulation(float deltaTime); + void integrate(float timeStep); + void setupGpuAabbsFull(); + + 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); + //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 reset(); + + 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); + + cl_mem getBodyBuffer(); + + int getNumBodies() const; + +}; + +#endif //B3_GPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h new file mode 100644 index 0000000000..5ac92f97d6 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h @@ -0,0 +1,73 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#ifndef B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H +#define B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H + +#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +#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_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<b3GpuGenericConstraint>* m_gpuConstraints; + b3AlignedObjectArray<b3GpuGenericConstraint> m_cpuConstraints; + + b3AlignedObjectArray<b3TypedConstraint*> m_joints; + int m_constraintUid; + class b3GpuNarrowPhase* m_narrowphase; + b3Vector3 m_gravity; + + b3Config m_config; +}; + +#endif //B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h new file mode 100644 index 0000000000..f2a61801ac --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h @@ -0,0 +1,228 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//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" + +#include "Bullet3Common/b3AlignedAllocator.h" +#include "Bullet3Common/b3TransformUtil.h" + +///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 // + + + +///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_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; + }; + + int padding[3]; + + /* + void setWorldTransform(const b3Transform& worldTransform) + { + m_worldTransform = worldTransform; + } + + const b3Transform& getWorldTransform() const + { + return m_worldTransform; + } + */ + 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); + else + velocity.setValue(0,0,0); + } + + B3_FORCE_INLINE void getAngularVelocity(b3Vector3& angVel) const + { + if (m_originalBody) + angVel =m_angularVelocity+m_deltaAngularVelocity; + else + 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) + { + if (m_originalBody) + { + 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) + { + if (m_originalBody) + { + m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + const b3Vector3& getDeltaLinearVelocity() const + { + return m_deltaLinearVelocity; + } + + const b3Vector3& getDeltaAngularVelocity() const + { + return m_deltaAngularVelocity; + } + + const b3Vector3& getPushVelocity() const + { + return m_pushVelocity; + } + + const b3Vector3& getTurnVelocity() const + { + return m_turnVelocity; + } + + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + b3Vector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + b3Vector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const b3Vector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const b3Vector3& internalGetInvMass() const + { + return m_invMass; + } + + void internalSetInvMass(const b3Vector3& invMass) + { + m_invMass = invMass; + } + + b3Vector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + b3Vector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + B3_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const + { + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + } + + B3_FORCE_INLINE void internalGetAngularVelocity(b3Vector3& angVel) const + { + 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) + { + //if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + + void writebackVelocity() + { + //if (m_originalBody>=0) + { + m_linearVelocity +=m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; + + //m_originalBody->setCompanionId(-1); + } + } + + + void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp) + { + (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) + { + // 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 + + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h new file mode 100644 index 0000000000..60d235baab --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h @@ -0,0 +1,82 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans http://github.com/erwincoumans/bullet3 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +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" +#include "Bullet3Common/b3AlignedObjectArray.h" + +//#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_DECLARE_ALIGNED_ALLOCATOR(); + + b3Vector3 m_relpos1CrossNormal; + b3Vector3 m_contactNormal; + + 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; + 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; + }; + + int m_overrideNumSolverIterations; + int m_frictionIndex; + int m_solverBodyIdA; + int m_solverBodyIdB; + + + enum b3SolverConstraintType + { + B3_SOLVER_CONTACT_1D = 0, + B3_SOLVER_FRICTION_1D + }; +}; + +typedef b3AlignedObjectArray<b3GpuSolverConstraint> b3GpuConstraintArray; + + +#endif //B3_GPU_SOLVER_CONSTRAINT_H + + + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp new file mode 100644 index 0000000000..20bf6d47c5 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.cpp @@ -0,0 +1,1225 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Takahiro Harada + + +#include "b3Solver.h" + +///useNewBatchingKernel is a rewritten kernel using just a single thread of the warp, for experiments +bool useNewBatchingKernel = true; +bool gConvertConstraintOnCpu = false; + +#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" +#define B3_SOLVER_FRICTION_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl" +#define B3_BATCHING_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl" +#define B3_BATCHING_NEW_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl" + +#include "Bullet3Dynamics/shared/b3ConvertConstraint4.h" + +#include "kernels/solverSetup.h" +#include "kernels/solverSetup2.h" + +#include "kernels/solveContact.h" +#include "kernels/solveFriction.h" + +#include "kernels/batchingKernels.h" +#include "kernels/batchingKernelsNew.h" + + +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3Common/b3Vector3.h" + +struct SolverDebugInfo +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + int m_valInt4; + int m_valInt5; + int m_valInt6; + int m_valInt7; + + int m_valInt8; + int m_valInt9; + int m_valInt10; + int m_valInt11; + + int m_valInt12; + int m_valInt13; + int m_valInt14; + int m_valInt15; + + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +}; + + + + +class SolverDeviceInl +{ +public: + struct ParallelSolveData + { + b3OpenCLArray<unsigned int>* m_numConstraints; + b3OpenCLArray<unsigned int>* m_offsets; + }; +}; + + + +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_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 ); + + 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->resize(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=""; + + + + 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); + b3Assert(solveContactProg); + + 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); + b3Assert(solverSetup2Prog); + + + 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 ); + b3Assert(m_solveFrictionKernel); + + 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 ); + b3Assert(m_contactToConstraintKernel); + + 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 ); + b3Assert(m_reorderContactKernel); + + + 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, 0, &pErrNum,additionalMacros, B3_BATCHING_PATH,true); + b3Assert(batchingProg); + + 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); + b3Assert(batchingNewProg); + + 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; + delete m_numConstraints; + delete m_sortDataBuffer; + delete m_contactBuffer2; + + delete m_sort32; + 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); + +} + + + + +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++) + { + // dont necessary because this makes change to 0 + 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 ); + + 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] ); + 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; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + if( JACOBI ) + { + dLinVelA += linImp0; + dAngVelA += angImp0; + dLinVelB += linImp1; + dAngVelB += angImp1; + } + else + { + linVelA += linImp0; + angVelA += angImp0; + linVelB += linImp1; + angVelB += angImp1; + } + } + } + + 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; + + 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] ); +#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]; + + { + 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; +#ifdef _WIN32 + b3Assert(_finite(linImp0.getX())); + b3Assert(_finite(linImp1.getX())); +#endif + 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 ); + + angVelA -= (angNA*0.1f)*n; + angVelB -= (angNB*0.1f)*n; + } + } + + } +/* + b3AlignedObjectArray<b3RigidBodyData>& m_bodies; + b3AlignedObjectArray<b3InertiaData>& m_shapes; + b3AlignedObjectArray<b3GpuConstraint4>& m_constraints; + b3AlignedObjectArray<int>* m_batchSizes; + int m_cellIndex; + int m_curWgidx; + int m_start; + int m_nConstraints; + bool m_solveFriction; + int m_maxNumBatches; + */ + +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; } + + void run(int tIdx) + { + int offset = 0; + for (int ii=0;ii<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++) + { + int i = m_start + offset+jj; + int batchId = m_constraints[i].m_batchIdx; + 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; + b3RigidBodyData& bodyA = m_bodies[aIdx]; + b3RigidBodyData& bodyB = m_bodies[bIdx]; + + 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}; + + 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 sum = 0; + for(int j=0; j<4; j++) + { + sum +=m_constraints[i].m_appliedRambdaDt[j]; + } + frictionCoeff = 0.7f; + for(int j=0; j<4; j++) + { + 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 ); + + } + } + offset+=numInBatch; + + + } +/* 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++) + { + + int i = m_start + ic; + if (m_constraints[i].m_batchIdx != bb) + continue; + + 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; + b3RigidBodyData& bodyA = m_bodies[aIdx]; + b3RigidBodyData& bodyB = m_bodies[bIdx]; + + 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}; + + 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 sum = 0; + for(int j=0; j<4; j++) + { + sum +=m_constraints[i].m_appliedRambdaDt[j]; + } + frictionCoeff = 0.7f; + for(int j=0; j<4; j++) + { + 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 ); + + } + } + } + */ + + + + } + + b3AlignedObjectArray<b3RigidBodyData>& m_bodies; + b3AlignedObjectArray<b3InertiaData>& m_shapes; + b3AlignedObjectArray<b3GpuConstraint4>& m_constraints; + b3AlignedObjectArray<int>* m_batchSizes; + int m_cellIndex; + int m_curWgidx; + int m_start; + int m_nConstraints; + bool m_solveFriction; + int m_maxNumBatches; +}; + + +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; + int nSplitY = B3_SOLVER_N_SPLIT_Y; + int numWorkgroups = B3_SOLVER_N_CELLS/B3_SOLVER_N_BATCHES; + for (int z=0;z<4;z++) + { + for (int y=0;y<4;y++) + { + for (int x=0;x<4;x++) + { + int newIndex = (x+y*nSplitX+z*nSplitX*nSplitY); + // printf("newIndex=%d\n",newIndex); + + int zIdx = newIndex/(nSplitX*nSplitY); + int remain = newIndex%(nSplitX*nSplitY); + int yIdx = remain/nSplitX; + int xIdx = remain%nSplitX; + // printf("newIndex=%d\n",newIndex); + } + } + } + + //for (int wgIdx=numWorkgroups-1;wgIdx>=0;wgIdx--) + for (int cellBatch=0;cellBatch<B3_SOLVER_N_BATCHES;cellBatch++) + { + 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 zIdx = newIndex/(nSplitX*nSplitY); + int remain = newIndex%(nSplitX*nSplitY); + int yIdx = remain/nSplitX; + int xIdx = remain%nSplitX; + */ + int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY); + // printf("wgIdx %d: xIdx=%d, yIdx=%d, zIdx=%d, cellIdx=%d, cell Batch %d\n",wgIdx,xIdx,yIdx,zIdx,cellIdx,cellBatch); + } + } + } +#endif + + b3AlignedObjectArray<b3RigidBodyData> bodyNative; + bodyBuf->copyToHost(bodyNative); + b3AlignedObjectArray<b3InertiaData> shapeNative; + shapeBuf->copyToHost(shapeNative); + b3AlignedObjectArray<b3GpuConstraint4> constraintNative; + constraint->copyToHost(constraintNative); + + b3AlignedObjectArray<unsigned int> numConstraintsHost; + m_numConstraints->copyToHost(numConstraintsHost); + + //printf("------------------------\n"); + b3AlignedObjectArray<unsigned int> offsetsHost; + m_offsets->copyToHost(offsetsHost); + static int frame=0; + bool useBatches=true; + if (useBatches) + { + for(int iter=0; iter<m_nIterations; iter++) + { + 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; + //printf("cell Batch %d\n",cellBatch); + b3AlignedObjectArray<int> usedBodies[B3_SOLVER_N_CELLS]; + 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++) + { + 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"); + } + + 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; + + 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 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; + + 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 ) + continue; + + //printf("yIdx=%d\n",yIdx); + + const int start = offsetsHost[cellIdx]; + int numConstraintsInCell = numConstraintsHost[cellIdx]; + // const int end = start + numConstraintsInCell; + + SolveTask task( bodyNative, shapeNative, constraintNative, start, numConstraintsInCell,maxNumBatches, 0,0,batchSizes,cellIdx); + task.m_solveFriction = true; + task.run(0); + + } + } + } + + + } else + { + for(int iter=0; iter<m_nIterations; iter++) + { + 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++) + { + SolveTask task( bodyNative, shapeNative, constraintNative, 0, n ,maxNumBatches,0,0,0,0); + task.m_solveFriction = true; + task.run(0); + } + } + + bodyBuf->copyFromHost(bodyNative); + 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 + ) +{ +// 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; + + b3AlignedObjectArray<unsigned int> gN; + m_numConstraints->copyToHost(gN); + b3AlignedObjectArray<unsigned int> gOffsets; + m_offsets->copyToHost(gOffsets); + int nSplitX = B3_SOLVER_N_SPLIT_X; + int nSplitY = B3_SOLVER_N_SPLIT_Y; + +// int bIdx = batchId; + + b3AlignedObjectArray<b3GpuConstraint4> cpuConstraints; + constraint->copyToHost(cpuConstraints); + + printf("batch = %d\n", batchId); + + int numWorkgroups = nn/B3_SOLVER_N_BATCHES; + b3AlignedObjectArray<int> usedBodies; + + + 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 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++) + { + b3GpuConstraint4& constraint = cpuConstraints[c]; + //printf("constraint (%d,%d)\n", constraint.m_bodyA,constraint.m_bodyB); + if (usedBodies.findLinearSearch(constraint.m_bodyA)< usedBodies.size()) + { + printf("error?\n"); + } + if (usedBodies.findLinearSearch(constraint.m_bodyB)< usedBodies.size()) + { + printf("error?\n"); + } + } + + 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; + +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 ); + { + + const int nn = B3_SOLVER_N_CELLS; + + cdata.x = 0; + cdata.y = maxNumBatches;//250; + + + int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES; +#ifdef DEBUG_ME + 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 ib=0; ib<B3_SOLVER_N_BATCHES; ib++) + { + + if (verify) + { + checkConstraintBatch(bodyBuf,shapeBuf,constraint,m_numConstraints,m_offsets,ib); + } + +#ifdef DEBUG_ME + memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems); + gpuDebugInfo.write(debugInfo,numWorkItems); +#endif + + + cdata.z = ib; + + + 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() ) +#ifdef DEBUG_ME + , b3BufferInfoCL(&gpuDebugInfo) +#endif + }; + + + + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + //launcher.setConst( cdata.x ); + 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 ); + + +#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); + } + +#endif + + +#ifdef DEBUG_ME + clFinish(m_queue); + gpuDebugInfo.read(debugInfo,numWorkItems); + clFinish(m_queue); + for (int i=0;i<numWorkItems;i++) + { + if (debugInfo[i].m_valInt2>0) + { + printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2); + } + + if (debugInfo[i].m_valInt3>0) + { + printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3); + } + } +#endif //DEBUG_ME + + + } + } + + clFinish(m_queue); + + + } + + cdata.x = 1; + bool applyFriction=true; + if (applyFriction) + { + B3_PROFILE("m_batchSolveKernel iterations2"); + for(int iter=0; iter<m_nIterations; iter++) + { + 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() ) +#ifdef DEBUG_ME + ,b3BufferInfoCL(&gpuDebugInfo) +#endif //DEBUG_ME + }; + 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; + 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 ); + } + } + clFinish(m_queue); + + } +#ifdef DEBUG_ME + delete[] debugInfo; +#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 ) +{ +// b3OpenCLArray<b3GpuConstraint4>* constraintNative =0; + contactCOut->resize(nContacts); + struct CB + { + int m_nContacts; + float m_dt; + float m_positionDrift; + float m_positionConstraintCoeff; + }; + + { + + 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); + + b3AlignedObjectArray<b3Contact4> gContact; + contactsIn->copyToHost(gContact); + + b3AlignedObjectArray<b3InertiaData> gShapes; + shapeBuf->copyToHost(gShapes); + + b3AlignedObjectArray<b3GpuConstraint4> gConstraintOut; + gConstraintOut.resize(nContacts); + + B3_PROFILE("cpu contactToConstraintKernel"); + for (int gIdx=0;gIdx<nContacts;gIdx++) + { + int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); + int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); + + b3Float4 posA = gBodies[aIdx].m_pos; + b3Float4 linVelA = gBodies[aIdx].m_linVel; + b3Float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + b3Mat3x3 invInertiaA = gShapes[aIdx].m_initInvInertia; + + b3Float4 posB = gBodies[bIdx].m_pos; + b3Float4 linVelB = gBodies[bIdx].m_linVel; + b3Float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + b3Mat3x3 invInertiaB = gShapes[bIdx].m_initInvInertia; + + 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 ); + + cs.m_batchIdx = gContact[gIdx].m_batchIdx; + + gConstraintOut[gIdx] = (b3GpuConstraint4&)cs; + } + + contactCOut->copyFromHost(gConstraintOut); + + } 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) ); + //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); + + } + } + + +} + +/* +void b3Solver::sortContacts( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, + b3OpenCLArray<b3Contact4>* contactsIn, void* additionalData, + int nContacts, const b3Solver::ConstraintCfg& cfg ) +{ + + + + const int sortAlignment = 512; // todo. get this out of sort + if( cfg.m_enableParallelSolve ) + { + + + int sortSize = NEXTMULTIPLEOF( nContacts, sortAlignment ); + + b3OpenCLArray<unsigned int>* countsNative = m_numConstraints;//BufferUtils::map<TYPE_CL, false>( data->m_device, &countsHost ); + b3OpenCLArray<unsigned int>* offsetsNative = m_offsets;//BufferUtils::map<TYPE_CL, false>( data->m_device, &offsetsHost ); + + { // 2. set cell idx + struct CB + { + int m_nContacts; + int m_staticIdx; + float m_scale; + int m_nSplit; + }; + + b3Assert( sortSize%64 == 0 ); + CB cdata; + cdata.m_nContacts = nContacts; + cdata.m_staticIdx = cfg.m_staticIdx; + cdata.m_scale = 1.f/(N_OBJ_PER_SPLIT*cfg.m_averageExtent); + cdata.m_nSplit = B3_SOLVER_N_SPLIT; + + + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) }; + b3LauncherCL launcher( m_queue, m_setSortDataKernel ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( sortSize, 64 ); + } + + { // 3. sort by cell idx + int n = B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT; + int sortBit = 32; + //if( n <= 0xffff ) sortBit = 16; + //if( n <= 0xff ) sortBit = 8; + m_sort32->execute(*m_sortDataBuffer,sortSize); + } + { // 4. find entries + m_search->execute( *m_sortDataBuffer, nContacts, *countsNative, B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT, b3BoundSearchCL::COUNT); + + m_scan->execute( *countsNative, *offsetsNative, B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT ); + } + + { // 5. sort constraints by cellIdx + // todo. preallocate this +// b3Assert( contactsIn->getType() == TYPE_HOST ); +// b3OpenCLArray<b3Contact4>* out = BufferUtils::map<TYPE_CL, false>( data->m_device, contactsIn ); // copying contacts to this buffer + + { + + + b3Int4 cdata; cdata.x = nContacts; + b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( m_contactBuffer->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) }; + b3LauncherCL launcher( m_queue, m_reorderContactKernel ); + launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) ); + launcher.setConst( cdata ); + launcher.launch1D( nContacts, 64 ); + } +// BufferUtils::unmap<true>( out, contactsIn, nContacts ); + } + } + + +} + +*/ +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; + { + 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); +#endif + + + +#if 0 + b3BufferInfoCL bInfo[] = { + b3BufferInfoCL( contacts->getBufferCL() ), + b3BufferInfoCL( m_contactBuffer2->getBufferCL()), + b3BufferInfoCL( nNative->getBufferCL() ), + b3BufferInfoCL( offsetsNative->getBufferCL() ), +#ifdef BATCH_DEBUG + , b3BufferInfoCL(&gpuDebugInfo) +#endif + }; +#endif + + + + { + m_batchSizes.resize(nNative->size()); + B3_PROFILE("batchingKernel"); + //b3LauncherCL launcher( m_queue, m_batchingKernel); + cl_kernel k = useNewBatchingKernel ? m_batchingKernelNew : m_batchingKernel; + + b3LauncherCL launcher( m_queue, k,"*batchingKernel"); + if (!useNewBatchingKernel ) + { + launcher.setBuffer( contacts->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 ); + //clFinish(m_queue); + //b3AlignedObjectArray<int> batchSizesCPU; + //m_batchSizes.copyToHost(batchSizesCPU); + //printf(".\n"); + } + +#ifdef BATCH_DEBUG + aaaa + b3Contact4* hostContacts = new b3Contact4[nContacts]; + m_contactBuffer->read(hostContacts,nContacts); + clFinish(m_queue); + + gpuDebugInfo.read(debugInfo,numWorkItems); + clFinish(m_queue); + + for (int i=0;i<numWorkItems;i++) + { + if (debugInfo[i].m_valInt1>0) + { + printf("catch\n"); + } + if (debugInfo[i].m_valInt2>0) + { + printf("catch22\n"); + } + + if (debugInfo[i].m_valInt3>0) + { + printf("catch666\n"); + } + + if (debugInfo[i].m_valInt4>0) + { + printf("catch777\n"); + } + } + delete[] debugInfo; +#endif //BATCH_DEBUG + + } + +// copy buffer to buffer + //b3Assert(m_contactBuffer->size()==nContacts); + //contacts->copyFromOpenCLArray( *m_contactBuffer); + //clFinish(m_queue);//needed? + + + +} + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h new file mode 100644 index 0000000000..b37f2f1bec --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/b3Solver.h @@ -0,0 +1,126 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Takahiro Harada + + +#ifndef __ADL_SOLVER_H +#define __ADL_SOLVER_H + +#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h" +#include "b3GpuConstraint4.h" + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" + +#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h" + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" + + +#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_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; + }; + +}; + +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; + + class b3RadixSort32CL* m_sort32; + class b3BoundSearchCL* m_search; + class b3PrefixScanCL* m_scan; + + b3OpenCLArray<b3SortData>* m_sortDataBuffer; + b3OpenCLArray<b3Contact4>* m_contactBuffer2; + + enum + { + DYNAMIC_CONTACT_ALLOCATION_THRESHOLD = 2000000, + }; + + + + + b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity); + + virtual ~b3Solver(); + + void solveContactConstraint( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* inertiaBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches); + + void solveContactConstraintHost( b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf, + b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches, b3AlignedObjectArray<int>* batchSizes); + + + 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 diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl new file mode 100644 index 0000000000..3b891b863d --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl @@ -0,0 +1,353 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Takahiro Harada + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile __global int* +#endif + + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +#define WG_SIZE 64 + + + + + +typedef struct +{ + int m_n; + int m_start; + int m_staticIdx; + int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_a; + int m_b; + u32 m_idx; +}Elem; + +#define STACK_SIZE (WG_SIZE*10) +//#define STACK_SIZE (WG_SIZE) +#define RING_SIZE 1024 +#define RING_SIZE_MASK (RING_SIZE-1) +#define CHECK_SIZE (WG_SIZE) + + +#define GET_RING_CAPACITY (RING_SIZE - ldsRingEnd) +#define RING_END ldsTmp + +u32 readBuf(__local u32* buff, int idx) +{ + idx = idx % (32*CHECK_SIZE); + int bitIdx = idx%32; + int bufIdx = idx/32; + return buff[bufIdx] & (1<<bitIdx); +} + +void writeBuf(__local u32* buff, int idx) +{ + idx = idx % (32*CHECK_SIZE); + int bitIdx = idx%32; + int bufIdx = idx/32; +// buff[bufIdx] |= (1<<bitIdx); + atom_or( &buff[bufIdx], (1<<bitIdx) ); +} + +u32 tryWrite(__local u32* buff, int idx) +{ + idx = idx % (32*CHECK_SIZE); + int bitIdx = idx%32; + int bufIdx = idx/32; + u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) ); + return ((ans >> bitIdx)&1) == 0; +} + +// batching on the GPU +__kernel void CreateBatches( __global const struct b3Contact4Data* gConstraints, __global struct b3Contact4Data* gConstraintsOut, + __global const u32* gN, __global const u32* gStart, __global int* batchSizes, + int m_staticIdx ) +{ + __local u32 ldsStackIdx[STACK_SIZE]; + __local u32 ldsStackEnd; + __local Elem ldsRingElem[RING_SIZE]; + __local u32 ldsRingEnd; + __local u32 ldsTmp; + __local u32 ldsCheckBuffer[CHECK_SIZE]; + __local u32 ldsFixedBuffer[CHECK_SIZE]; + __local u32 ldsGEnd; + __local u32 ldsDstEnd; + + int wgIdx = GET_GROUP_IDX; + int lIdx = GET_LOCAL_IDX; + + const int m_n = gN[wgIdx]; + const int m_start = gStart[wgIdx]; + + if( lIdx == 0 ) + { + ldsRingEnd = 0; + ldsGEnd = 0; + ldsStackEnd = 0; + ldsDstEnd = m_start; + } + + + +// while(1) +//was 250 + int ie=0; + int maxBatch = 0; + for(ie=0; ie<50; ie++) + { + ldsFixedBuffer[lIdx] = 0; + + for(int giter=0; giter<4; giter++) + { + int ringCap = GET_RING_CAPACITY; + + // 1. fill ring + if( ldsGEnd < m_n ) + { + while( ringCap > WG_SIZE ) + { + if( ldsGEnd >= m_n ) break; + if( lIdx < ringCap - WG_SIZE ) + { + int srcIdx; + AtomInc1( ldsGEnd, srcIdx ); + if( srcIdx < m_n ) + { + int dstIdx; + AtomInc1( ldsRingEnd, dstIdx ); + + int a = gConstraints[m_start+srcIdx].m_bodyAPtrAndSignBit; + int b = gConstraints[m_start+srcIdx].m_bodyBPtrAndSignBit; + ldsRingElem[dstIdx].m_a = (a>b)? b:a; + ldsRingElem[dstIdx].m_b = (a>b)? a:b; + ldsRingElem[dstIdx].m_idx = srcIdx; + } + } + ringCap = GET_RING_CAPACITY; + } + } + + GROUP_LDS_BARRIER; + + // 2. fill stack + __local Elem* dst = ldsRingElem; + if( lIdx == 0 ) RING_END = 0; + + int srcIdx=lIdx; + int end = ldsRingEnd; + + { + for(int ii=0; ii<end; ii+=WG_SIZE, srcIdx+=WG_SIZE) + { + Elem e; + if(srcIdx<end) e = ldsRingElem[srcIdx]; + bool done = (srcIdx<end)?false:true; + + for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) ldsCheckBuffer[lIdx] = 0; + + if( !done ) + { + int aUsed = readBuf( ldsFixedBuffer, abs(e.m_a)); + int bUsed = readBuf( ldsFixedBuffer, abs(e.m_b)); + + if( aUsed==0 && bUsed==0 ) + { + int aAvailable=1; + int bAvailable=1; + int ea = abs(e.m_a); + int eb = abs(e.m_b); + + bool aStatic = (e.m_a<0) ||(ea==m_staticIdx); + bool bStatic = (e.m_b<0) ||(eb==m_staticIdx); + + if (!aStatic) + aAvailable = tryWrite( ldsCheckBuffer, ea ); + if (!bStatic) + bAvailable = tryWrite( ldsCheckBuffer, eb ); + + //aAvailable = aStatic? 1: aAvailable; + //bAvailable = bStatic? 1: bAvailable; + + bool success = (aAvailable && bAvailable); + if(success) + { + + if (!aStatic) + writeBuf( ldsFixedBuffer, ea ); + if (!bStatic) + writeBuf( ldsFixedBuffer, eb ); + } + done = success; + } + } + + // put it aside + if(srcIdx<end) + { + if( done ) + { + int dstIdx; AtomInc1( ldsStackEnd, dstIdx ); + if( dstIdx < STACK_SIZE ) + ldsStackIdx[dstIdx] = e.m_idx; + else{ + done = false; + AtomAdd( ldsStackEnd, -1 ); + } + } + if( !done ) + { + int dstIdx; AtomInc1( RING_END, dstIdx ); + dst[dstIdx] = e; + } + } + + // if filled, flush + if( ldsStackEnd == STACK_SIZE ) + { + for(int i=lIdx; i<STACK_SIZE; i+=WG_SIZE) + { + int idx = m_start + ldsStackIdx[i]; + int dstIdx; AtomInc1( ldsDstEnd, dstIdx ); + gConstraintsOut[ dstIdx ] = gConstraints[ idx ]; + gConstraintsOut[ dstIdx ].m_batchIdx = ie; + } + if( lIdx == 0 ) ldsStackEnd = 0; + + //for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) + ldsFixedBuffer[lIdx] = 0; + } + } + } + + if( lIdx == 0 ) ldsRingEnd = RING_END; + } + + GROUP_LDS_BARRIER; + + for(int i=lIdx; i<ldsStackEnd; i+=WG_SIZE) + { + int idx = m_start + ldsStackIdx[i]; + int dstIdx; AtomInc1( ldsDstEnd, dstIdx ); + gConstraintsOut[ dstIdx ] = gConstraints[ idx ]; + gConstraintsOut[ dstIdx ].m_batchIdx = ie; + } + + // in case it couldn't consume any pair. Flush them + // todo. Serial batch worth while? + if( ldsStackEnd == 0 ) + { + for(int i=lIdx; i<ldsRingEnd; i+=WG_SIZE) + { + int idx = m_start + ldsRingElem[i].m_idx; + int dstIdx; AtomInc1( ldsDstEnd, dstIdx ); + gConstraintsOut[ dstIdx ] = gConstraints[ idx ]; + int curBatch = 100+i; + if (maxBatch < curBatch) + maxBatch = curBatch; + + gConstraintsOut[ dstIdx ].m_batchIdx = curBatch; + + } + GROUP_LDS_BARRIER; + if( lIdx == 0 ) ldsRingEnd = 0; + } + + if( lIdx == 0 ) ldsStackEnd = 0; + + GROUP_LDS_BARRIER; + + // termination + if( ldsGEnd == m_n && ldsRingEnd == 0 ) + break; + } + + if( lIdx == 0 ) + { + if (maxBatch < ie) + maxBatch=ie; + batchSizes[wgIdx]=maxBatch; + } + +} + + + + + + + + + + + + + + + + + + + + + + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h new file mode 100644 index 0000000000..150eedc94b --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h @@ -0,0 +1,388 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl new file mode 100644 index 0000000000..ba1b66d2c3 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl @@ -0,0 +1,231 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile __global int* +#endif + +#define SIMD_WIDTH 64 + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +#define WG_SIZE 64 + + + + + +typedef struct +{ + int m_n; + int m_start; + int m_staticIdx; + int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_a; + int m_b; + u32 m_idx; +}Elem; + + + + + +// batching on the GPU +__kernel void CreateBatchesBruteForce( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, int m_staticIdx ) +{ + int wgIdx = GET_GROUP_IDX; + int lIdx = GET_LOCAL_IDX; + + const int m_n = gN[wgIdx]; + const int m_start = gStart[wgIdx]; + + if( lIdx == 0 ) + { + for (int i=0;i<m_n;i++) + { + int srcIdx = i+m_start; + int batchIndex = i; + gConstraints[ srcIdx ].m_batchIdx = batchIndex; + } + } +} + + +#define CHECK_SIZE (WG_SIZE) + + + + +u32 readBuf(__local u32* buff, int idx) +{ + idx = idx % (32*CHECK_SIZE); + int bitIdx = idx%32; + int bufIdx = idx/32; + return buff[bufIdx] & (1<<bitIdx); +} + +void writeBuf(__local u32* buff, int idx) +{ + idx = idx % (32*CHECK_SIZE); + int bitIdx = idx%32; + int bufIdx = idx/32; + buff[bufIdx] |= (1<<bitIdx); + //atom_or( &buff[bufIdx], (1<<bitIdx) ); +} + +u32 tryWrite(__local u32* buff, int idx) +{ + idx = idx % (32*CHECK_SIZE); + int bitIdx = idx%32; + int bufIdx = idx/32; + u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) ); + return ((ans >> bitIdx)&1) == 0; +} + + +// batching on the GPU +__kernel void CreateBatchesNew( __global struct b3Contact4Data* gConstraints, __global const u32* gN, __global const u32* gStart, __global int* batchSizes, int staticIdx ) +{ + int wgIdx = GET_GROUP_IDX; + int lIdx = GET_LOCAL_IDX; + const int numConstraints = gN[wgIdx]; + const int m_start = gStart[wgIdx]; + b3Contact4Data_t tmp; + + __local u32 ldsFixedBuffer[CHECK_SIZE]; + + + + + + if( lIdx == 0 ) + { + + + __global struct b3Contact4Data* cs = &gConstraints[m_start]; + + + int numValidConstraints = 0; + int batchIdx = 0; + + while( numValidConstraints < numConstraints) + { + int nCurrentBatch = 0; + // clear flag + + for(int i=0; i<CHECK_SIZE; i++) + ldsFixedBuffer[i] = 0; + + for(int i=numValidConstraints; i<numConstraints; i++) + { + + int bodyAS = cs[i].m_bodyAPtrAndSignBit; + int bodyBS = cs[i].m_bodyBPtrAndSignBit; + int bodyA = abs(bodyAS); + int bodyB = abs(bodyBS); + bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx; + bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx; + int aUnavailable = aIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyA); + int bUnavailable = bIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyB); + + if( aUnavailable==0 && bUnavailable==0 ) // ok + { + if (!aIsStatic) + { + writeBuf( ldsFixedBuffer, bodyA ); + } + if (!bIsStatic) + { + writeBuf( ldsFixedBuffer, bodyB ); + } + + cs[i].m_batchIdx = batchIdx; + + if (i!=numValidConstraints) + { + + tmp = cs[i]; + cs[i] = cs[numValidConstraints]; + cs[numValidConstraints] = tmp; + + + } + + numValidConstraints++; + + nCurrentBatch++; + if( nCurrentBatch == SIMD_WIDTH) + { + nCurrentBatch = 0; + for(int i=0; i<CHECK_SIZE; i++) + ldsFixedBuffer[i] = 0; + + } + } + }//for + batchIdx ++; + }//while + + batchSizes[wgIdx] = batchIdx; + + }//if( lIdx == 0 ) + + //return batchIdx; +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h new file mode 100644 index 0000000000..1e5957adae --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h @@ -0,0 +1,291 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl new file mode 100644 index 0000000000..e22bc9bc33 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl @@ -0,0 +1,32 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + +#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h" + + + +__kernel void + integrateTransformsKernel( __global b3RigidBodyData_t* bodies,const int numNodes, float timeStep, float angularDamping, float4 gravityAcceleration) +{ + int nodeID = get_global_id(0); + + if( nodeID < numNodes) + { + integrateSingleTransform(bodies,nodeID, timeStep, angularDamping,gravityAcceleration); + } +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h new file mode 100644 index 0000000000..a5a432947c --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h @@ -0,0 +1,433 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl new file mode 100644 index 0000000000..7f5dabe274 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl @@ -0,0 +1,877 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#define B3_CONSTRAINT_FLAG_ENABLED 1 + +#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3 +#define B3_GPU_FIXED_CONSTRAINT_TYPE 4 + +#define MOTIONCLAMP 100000 //unused, for debugging/safety in case constraint solver fails +#define B3_INFINITY 1e30f + +#define mymake_float4 (float4) + + +__inline float dot3F4(float4 a, float4 b) +{ + float4 a1 = mymake_float4(a.xyz,0.f); + float4 b1 = mymake_float4(b.xyz,0.f); + return dot(a1, b1); +} + + +typedef float4 Quaternion; + + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + + + +typedef struct +{ + Matrix3x3 m_invInertiaWorld; + Matrix3x3 m_initInvInertia; +} BodyInertia; + + +typedef struct +{ + Matrix3x3 m_basis;//orientation + float4 m_origin;//transform +}b3Transform; + +typedef struct +{ +// b3Transform m_worldTransformUnused; + float4 m_deltaLinearVelocity; + float4 m_deltaAngularVelocity; + float4 m_angularFactor; + float4 m_linearFactor; + float4 m_invMass; + float4 m_pushVelocity; + float4 m_turnVelocity; + float4 m_linearVelocity; + float4 m_angularVelocity; + + union + { + void* m_originalBody; + int m_originalBodyIndex; + }; + int padding[3]; + +} b3GpuSolverBody; + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + unsigned int m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} b3RigidBodyCL; + +typedef struct +{ + + float4 m_relpos1CrossNormal; + float4 m_contactNormal; + + float4 m_relpos2CrossNormal; + //float4 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal + + float4 m_angularComponentA; + float4 m_angularComponentB; + + float m_appliedPushImpulse; + float m_appliedImpulse; + int m_padding1; + int m_padding2; + float m_friction; + float m_jacDiagABInv; + float m_rhs; + float m_cfm; + + float m_lowerLimit; + float m_upperLimit; + float m_rhsPenetration; + int m_originalConstraint; + + + int m_overrideNumSolverIterations; + int m_frictionIndex; + int m_solverBodyIdA; + int m_solverBodyIdB; + +} b3SolverConstraint; + +typedef struct +{ + int m_bodyAPtrAndSignBit; + int m_bodyBPtrAndSignBit; + int m_originalConstraintIndex; + int m_batchId; +} b3BatchConstraint; + + + + + + +typedef struct +{ + int m_constraintType; + int m_rbA; + int m_rbB; + float m_breakingImpulseThreshold; + + float4 m_pivotInA; + float4 m_pivotInB; + Quaternion m_relTargetAB; + + int m_flags; + int m_padding[3]; +} b3GpuGenericConstraint; + + +/*b3Transform getWorldTransform(b3RigidBodyCL* rb) +{ + b3Transform newTrans; + newTrans.setOrigin(rb->m_pos); + newTrans.setRotation(rb->m_quat); + return newTrans; +}*/ + + + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float4 fastNormalize4(float4 v) +{ + v = mymake_float4(v.xyz,0.f); + return fast_normalize(v); +} + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + + +__inline void internalApplyImpulse(__global b3GpuSolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude) +{ + body->m_deltaLinearVelocity += linearComponent*impulseMagnitude*body->m_linearFactor; + body->m_deltaAngularVelocity += angularComponent*(impulseMagnitude*body->m_angularFactor); +} + + +void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c) +{ + float deltaImpulse = c->m_rhs-c->m_appliedImpulse*c->m_cfm; + float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity); + float deltaVel2Dotn = -dot3F4(c->m_contactNormal,body2->m_deltaLinearVelocity) + dot3F4(c->m_relpos2CrossNormal,body2->m_deltaAngularVelocity); + + deltaImpulse -= deltaVel1Dotn*c->m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn*c->m_jacDiagABInv; + + float sum = c->m_appliedImpulse + deltaImpulse; + if (sum < c->m_lowerLimit) + { + deltaImpulse = c->m_lowerLimit-c->m_appliedImpulse; + c->m_appliedImpulse = c->m_lowerLimit; + } + else if (sum > c->m_upperLimit) + { + deltaImpulse = c->m_upperLimit-c->m_appliedImpulse; + c->m_appliedImpulse = c->m_upperLimit; + } + else + { + 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); + +} + +__kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies, + __global b3BatchConstraint* batchConstraints, + __global b3SolverConstraint* rows, + __global unsigned int* numConstraintRowsInfo1, + __global unsigned int* rowOffsets, + __global b3GpuGenericConstraint* constraints, + int batchOffset, + int numConstraintsInBatch + ) +{ + int b = get_global_id(0); + if (b>=numConstraintsInBatch) + return; + + __global b3BatchConstraint* c = &batchConstraints[b+batchOffset]; + int originalConstraintIndex = c->m_originalConstraintIndex; + if (constraints[originalConstraintIndex].m_flags&B3_CONSTRAINT_FLAG_ENABLED) + { + int numConstraintRows = numConstraintRowsInfo1[originalConstraintIndex]; + int rowOffset = rowOffsets[originalConstraintIndex]; + for (int jj=0;jj<numConstraintRows;jj++) + { + __global b3SolverConstraint* constraint = &rows[rowOffset+jj]; + resolveSingleConstraintRowGeneric(&solverBodies[constraint->m_solverBodyIdA],&solverBodies[constraint->m_solverBodyIdB],constraint); + } + } +}; + +__kernel void initSolverBodies(__global b3GpuSolverBody* solverBodies,__global b3RigidBodyCL* bodiesCL, int numBodies) +{ + int i = get_global_id(0); + if (i>=numBodies) + return; + + __global b3GpuSolverBody* solverBody = &solverBodies[i]; + __global b3RigidBodyCL* bodyCL = &bodiesCL[i]; + + solverBody->m_deltaLinearVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_deltaAngularVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_pushVelocity = (float4)(0.f,0.f,0.f,0.f); + solverBody->m_invMass = (float4)(bodyCL->m_invMass,bodyCL->m_invMass,bodyCL->m_invMass,0.f); + solverBody->m_originalBodyIndex = i; + solverBody->m_angularFactor = (float4)(1,1,1,0); + solverBody->m_linearFactor = (float4) (1,1,1,0); + solverBody->m_linearVelocity = bodyCL->m_linVel; + solverBody->m_angularVelocity = bodyCL->m_angVel; +} + +__kernel void breakViolatedConstraintsKernel(__global b3GpuGenericConstraint* constraints, __global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, __global b3SolverConstraint* rows, int numConstraints) +{ + int cid = get_global_id(0); + if (cid>=numConstraints) + return; + int numRows = numConstraintRows[cid]; + if (numRows) + { + for (int i=0;i<numRows;i++) + { + int rowIndex = rowOffsets[cid]+i; + float breakingThreshold = constraints[cid].m_breakingImpulseThreshold; + if (fabs(rows[rowIndex].m_appliedImpulse) >= breakingThreshold) + { + constraints[cid].m_flags =0;//&= ~B3_CONSTRAINT_FLAG_ENABLED; + } + } + } +} + + + +__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, int numConstraints) +{ + int i = get_global_id(0); + if (i>=numConstraints) + return; + + __global b3GpuGenericConstraint* constraint = &constraints[i]; + + switch (constraint->m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + infos[i] = 3; + break; + } + case B3_GPU_FIXED_CONSTRAINT_TYPE: + { + infos[i] = 6; + break; + } + default: + { + } + } +} + +__kernel void initBatchConstraintsKernel(__global unsigned int* numConstraintRows, __global unsigned int* rowOffsets, + __global b3BatchConstraint* batchConstraints, + __global b3GpuGenericConstraint* constraints, + __global b3RigidBodyCL* bodies, + int numConstraints) +{ + int i = get_global_id(0); + if (i>=numConstraints) + return; + + int rbA = constraints[i].m_rbA; + int rbB = constraints[i].m_rbB; + + batchConstraints[i].m_bodyAPtrAndSignBit = bodies[rbA].m_invMass != 0.f ? rbA : -rbA; + batchConstraints[i].m_bodyBPtrAndSignBit = bodies[rbB].m_invMass != 0.f ? rbB : -rbB; + batchConstraints[i].m_batchId = -1; + batchConstraints[i].m_originalConstraintIndex = i; + +} + + + + +typedef struct +{ + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + float 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. + union + { + __global float4* m_J1linearAxisFloat4; + __global float* m_J1linearAxis; + }; + union + { + __global float4* m_J1angularAxisFloat4; + __global float* m_J1angularAxis; + + }; + union + { + __global float4* m_J2linearAxisFloat4; + __global float* m_J2linearAxis; + }; + union + { + __global float4* m_J2angularAxisFloat4; + __global float* m_J2angularAxis; + }; + // elements to jump from one row to the next in J's + int rowskip; + + // 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. + __global float* m_constraintError; + __global float* cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + __global float* m_lowerLimit; + __global float* 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. + __global int *findex; + // number of solver iterations + int m_numIterations; + + //damping of the velocity + float m_damping; +} b3GpuConstraintInfo2; + + +void getSkewSymmetricMatrix(float4 vecIn, __global float4* v0,__global float4* v1,__global float4* v2) +{ + *v0 = (float4)(0. ,-vecIn.z ,vecIn.y,0.f); + *v1 = (float4)(vecIn.z ,0. ,-vecIn.x,0.f); + *v2 = (float4)(-vecIn.y ,vecIn.x ,0.f,0.f); +} + + +void getInfo2Point2Point(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies) +{ + float4 posA = bodies[constraint->m_rbA].m_pos; + Quaternion rotA = bodies[constraint->m_rbA].m_quat; + + float4 posB = bodies[constraint->m_rbB].m_pos; + Quaternion rotB = 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; + + float4 a1 = qtRotate(rotA,constraint->m_pivotInA); + + { + __global float4* angular0 = (__global float4*)(info->m_J1angularAxis); + __global float4* angular1 = (__global float4*)(info->m_J1angularAxis+info->rowskip); + __global float4* angular2 = (__global float4*)(info->m_J1angularAxis+2*info->rowskip); + float4 a1neg = -a1; + getSkewSymmetricMatrix(a1neg,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; + } + + float4 a2 = qtRotate(rotB,constraint->m_pivotInB); + + { + // float4 a2n = -a2; + __global float4* angular0 = (__global float4*)(info->m_J2angularAxis); + __global float4* angular1 = (__global float4*)(info->m_J2angularAxis+info->rowskip); + __global float4* angular2 = (__global float4*)(info->m_J2angularAxis+2*info->rowskip); + getSkewSymmetricMatrix(a2,angular0,angular1,angular2); + } + + // set right hand side +// float currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; + float currERP = info->erp; + + float k = info->fps * currERP; + int j; + float4 result = a2 + posB - a1 - posA; + float* resultPtr = &result; + + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]); + } +} + +Quaternion nearest( Quaternion first, Quaternion qd) +{ + Quaternion diff,sum; + diff = first- qd; + sum = first + qd; + + if( dot(diff,diff) < dot(sum,sum) ) + return qd; + return (-qd); +} + +float b3Acos(float x) +{ + if (x<-1) + x=-1; + if (x>1) + x=1; + return acos(x); +} + +float getAngle(Quaternion orn) +{ + if (orn.w>=1.f) + orn.w=1.f; + float s = 2.f * b3Acos(orn.w); + return s; +} + +void calculateDiffAxisAngleQuaternion( Quaternion orn0,Quaternion orn1a,float4* axis,float* angle) +{ + Quaternion orn1 = nearest(orn0,orn1a); + + Quaternion dorn = qtMul(orn1,qtInvert(orn0)); + *angle = getAngle(dorn); + *axis = (float4)(dorn.x,dorn.y,dorn.z,0.f); + + //check for axis length + float len = dot3F4(*axis,*axis); + if (len < FLT_EPSILON*FLT_EPSILON) + *axis = (float4)(1,0,0,0); + else + *axis /= sqrt(len); +} + + + +void getInfo2FixedOrientation(__global b3GpuGenericConstraint* constraint,b3GpuConstraintInfo2* info,__global b3RigidBodyCL* bodies, int start_row) +{ + Quaternion worldOrnA = bodies[constraint->m_rbA].m_quat; + Quaternion worldOrnB = bodies[constraint->m_rbB].m_quat; + + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->m_J1angularAxis[start_index] = 1; + info->m_J1angularAxis[start_index + s + 1] = 1; + info->m_J1angularAxis[start_index + s*2+2] = 1; + if ( info->m_J2angularAxis) + { + info->m_J2angularAxis[start_index] = -1; + info->m_J2angularAxis[start_index + s+1] = -1; + info->m_J2angularAxis[start_index + s*2+2] = -1; + } + + float currERP = info->erp; + float k = info->fps * currERP; + float4 diff; + float angle; + float4 qrelCur = qtMul(worldOrnA,qtInvert(worldOrnB)); + + calculateDiffAxisAngleQuaternion(constraint->m_relTargetAB,qrelCur,&diff,&angle); + diff*=-angle; + + float* resultPtr = &diff; + + for (int j=0; j<3; j++) + { + info->m_constraintError[(3+j)*info->rowskip] = k * resultPtr[j]; + } + + +} + + +__kernel void writeBackVelocitiesKernel(__global b3RigidBodyCL* bodies,__global b3GpuSolverBody* solverBodies,int numBodies) +{ + int i = get_global_id(0); + if (i>=numBodies) + return; + + if (bodies[i].m_invMass) + { +// if (length(solverBodies[i].m_deltaLinearVelocity)<MOTIONCLAMP) + { + bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity; + } +// if (length(solverBodies[i].m_deltaAngularVelocity)<MOTIONCLAMP) + { + bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity; + } + } +} + + +__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, + __global unsigned int* infos, + __global unsigned int* constraintRowOffsets, + __global b3GpuGenericConstraint* constraints, + __global b3BatchConstraint* batchConstraints, + __global b3RigidBodyCL* bodies, + __global BodyInertia* inertias, + __global b3GpuSolverBody* solverBodies, + float timeStep, + float globalErp, + float globalCfm, + float globalDamping, + int globalNumIterations, + int numConstraints) +{ + + int i = get_global_id(0); + if (i>=numConstraints) + return; + + //for now, always initialize the batch info + int info1 = infos[i]; + + __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[constraintRowOffsets[i]]; + __global b3GpuGenericConstraint* constraint = &constraints[i]; + + __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA]; + __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB]; + + int solverBodyIdA = constraint->m_rbA; + int solverBodyIdB = constraint->m_rbB; + + __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA]; + __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB]; + + + if (rbA->m_invMass) + { + batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA; + } else + { +// if (!solverBodyIdA) +// m_staticIdx = 0; + batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA; + } + + if (rbB->m_invMass) + { + batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB; + } else + { +// if (!solverBodyIdB) +// m_staticIdx = 0; + batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB; + } + + if (info1) + { + 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++) + { +// memset(¤tConstraintRow[j],0,sizeof(b3SolverConstraint)); + currentConstraintRow[j].m_angularComponentA = (float4)(0,0,0,0); + currentConstraintRow[j].m_angularComponentB = (float4)(0,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 = (float4)(0,0,0,0); + currentConstraintRow[j].m_friction = 0.f; + currentConstraintRow[j].m_frictionIndex = 0; + currentConstraintRow[j].m_jacDiagABInv = 0.f; + currentConstraintRow[j].m_lowerLimit = 0.f; + currentConstraintRow[j].m_upperLimit = 0.f; + + currentConstraintRow[j].m_originalConstraint = i; + currentConstraintRow[j].m_overrideNumSolverIterations = 0; + currentConstraintRow[j].m_relpos1CrossNormal = (float4)(0,0,0,0); + currentConstraintRow[j].m_relpos2CrossNormal = (float4)(0,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; + currentConstraintRow[j].m_appliedPushImpulse = 0.f; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; + currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; + } + + bodyAPtr->m_deltaLinearVelocity = (float4)(0,0,0,0); + bodyAPtr->m_deltaAngularVelocity = (float4)(0,0,0,0); + bodyAPtr->m_pushVelocity = (float4)(0,0,0,0); + bodyAPtr->m_turnVelocity = (float4)(0,0,0,0); + bodyBPtr->m_deltaLinearVelocity = (float4)(0,0,0,0); + bodyBPtr->m_deltaAngularVelocity = (float4)(0,0,0,0); + bodyBPtr->m_pushVelocity = (float4)(0,0,0,0); + bodyBPtr->m_turnVelocity = (float4)(0,0,0,0); + + int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this + + + + + b3GpuConstraintInfo2 info2; + info2.fps = 1.f/timeStep; + info2.erp = globalErp; + info2.m_J1linearAxisFloat4 = ¤tConstraintRow->m_contactNormal; + info2.m_J1angularAxisFloat4 = ¤tConstraintRow->m_relpos1CrossNormal; + info2.m_J2linearAxisFloat4 = 0; + info2.m_J2angularAxisFloat4 = ¤tConstraintRow->m_relpos2CrossNormal; + info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this + + ///the size of b3SolverConstraint needs be a multiple of float +// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint)); + info2.m_constraintError = ¤tConstraintRow->m_rhs; + currentConstraintRow->m_cfm = globalCfm; + info2.m_damping = globalDamping; + info2.cfm = ¤tConstraintRow->m_cfm; + info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit; + info2.m_upperLimit = ¤tConstraintRow->m_upperLimit; + info2.m_numIterations = globalNumIterations; + + switch (constraint->m_constraintType) + { + case B3_GPU_POINT2POINT_CONSTRAINT_TYPE: + { + getInfo2Point2Point(constraint,&info2,bodies); + break; + } + case B3_GPU_FIXED_CONSTRAINT_TYPE: + { + getInfo2Point2Point(constraint,&info2,bodies); + + getInfo2FixedOrientation(constraint,&info2,bodies,3); + + break; + } + + default: + { + } + } + + ///finalize the constraint setup + for ( j=0;j<info1;j++) + { + __global b3SolverConstraint* solverConstraint = ¤tConstraintRow[j]; + + if (solverConstraint->m_upperLimit>=constraint->m_breakingImpulseThreshold) + { + solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold; + } + + if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold) + { + solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold; + } + +// solverConstraint->m_originalContactPoint = constraint; + + Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld; + { + + //float4 angularFactorA(1,1,1); + float4 ftorqueAxis1 = solverConstraint->m_relpos1CrossNormal; + solverConstraint->m_angularComponentA = mtMul1(invInertiaWorldA,ftorqueAxis1);//*angularFactorA; + } + + Matrix3x3 invInertiaWorldB= inertias[constraint->m_rbB].m_invInertiaWorld; + { + + float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal; + solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor(); + } + + { + //it is ok to use solverConstraint->m_contactNormal instead of -solverConstraint->m_contactNormal + //because it gets multiplied iMJlB + float4 iMJlA = solverConstraint->m_contactNormal*rbA->m_invMass; + float4 iMJaA = mtMul3(solverConstraint->m_relpos1CrossNormal,invInertiaWorldA); + float4 iMJlB = solverConstraint->m_contactNormal*rbB->m_invMass;//sign of normal? + float4 iMJaB = mtMul3(solverConstraint->m_relpos2CrossNormal,invInertiaWorldB); + + float sum = dot3F4(iMJlA,solverConstraint->m_contactNormal); + sum += dot3F4(iMJaA,solverConstraint->m_relpos1CrossNormal); + sum += dot3F4(iMJlB,solverConstraint->m_contactNormal); + sum += dot3F4(iMJaB,solverConstraint->m_relpos2CrossNormal); + float fsum = fabs(sum); + if (fsum>FLT_EPSILON) + { + solverConstraint->m_jacDiagABInv = 1.f/sum; + } else + { + solverConstraint->m_jacDiagABInv = 0.f; + } + } + + + ///fix rhs + ///todo: add force/torque accelerators + { + float rel_vel; + float vel1Dotn = dot3F4(solverConstraint->m_contactNormal,rbA->m_linVel) + dot3F4(solverConstraint->m_relpos1CrossNormal,rbA->m_angVel); + float vel2Dotn = -dot3F4(solverConstraint->m_contactNormal,rbB->m_linVel) + dot3F4(solverConstraint->m_relpos2CrossNormal,rbB->m_angVel); + + rel_vel = vel1Dotn+vel2Dotn; + + float restitution = 0.f; + float positionalError = solverConstraint->m_rhs;//already filled in by getConstraintInfo2 + float velocityError = restitution - rel_vel * info2.m_damping; + float penetrationImpulse = positionalError*solverConstraint->m_jacDiagABInv; + float velocityImpulse = velocityError *solverConstraint->m_jacDiagABInv; + solverConstraint->m_rhs = penetrationImpulse+velocityImpulse; + solverConstraint->m_appliedImpulse = 0.f; + + } + } + } +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h new file mode 100644 index 0000000000..d48ecf6ea6 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h @@ -0,0 +1,721 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl new file mode 100644 index 0000000000..5c4d62e4ec --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl @@ -0,0 +1,501 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Takahiro Harada + + +//#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define mymake_float4 (float4) +//#define make_float2 (float2) +//#define make_uint4 (uint4) +//#define make_int4 (int4) +//#define make_uint2 (uint2) +//#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// + + + + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = mymake_float4(a.xyz,0.f); + float4 b1 = mymake_float4(b.xyz,0.f); + return dot(a1, b1); +} + + + + +__inline +float4 normalize3(const float4 a) +{ + float4 n = mymake_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + + + + +/////////////////////////////////////// +// Matrix3x3 +/////////////////////////////////////// + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + + + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + + + + + + + +#define WG_SIZE 64 + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + +typedef struct +{ + Matrix3x3 m_invInertia; + Matrix3x3 m_initInvInertia; +} Shape; + +typedef struct +{ + float4 m_linear; + float4 m_worldPos[4]; + float4 m_center; + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + + float m_fJacCoeffInv[2]; + float m_fAppliedRambdaDt[2]; + + u32 m_bodyA; + u32 m_bodyB; + + int m_batchIdx; + u32 m_paddings[1]; +} Constraint4; + + + +typedef struct +{ + int m_nConstraints; + int m_start; + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_solveFriction; + int m_maxBatch; // long batch really kills the performance + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBufferBatchSolve; + +void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1); + +void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1) +{ + *linear = mymake_float4(-n.xyz,0.f); + *angular0 = -cross3(r0, n); + *angular1 = cross3(r1, n); +} + +float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 ); + +float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 ) +{ + return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1); +} + + +float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1, + float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1); + +float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1, + float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1) +{ + // linear0,1 are normlized + float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0; + float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0); + float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1; + float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1); + return -1.f/(jmj0+jmj1+jmj2+jmj3); +} + + +void solveContact(__global Constraint4* cs, + float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA, + float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB); + +void solveContact(__global Constraint4* cs, + float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA, + float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB) +{ + float minRambdaDt = 0; + float maxRambdaDt = FLT_MAX; + + for(int ic=0; ic<4; ic++) + { + if( cs->m_jacCoeffInv[ic] == 0.f ) continue; + + float4 angular0, angular1, linear; + float4 r0 = cs->m_worldPos[ic] - posA; + float4 r1 = cs->m_worldPos[ic] - posB; + setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 ); + + float rambdaDt = calcRelVel( cs->m_linear, -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 = max2( updated, minRambdaDt ); + updated = min2( updated, maxRambdaDt ); + rambdaDt = updated - prevSum; + cs->m_appliedRambdaDt[ic] = updated; + } + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + *linVelA += linImp0; + *angVelA += angImp0; + *linVelB += linImp1; + *angVelB += angImp1; + } +} + +void btPlaneSpace1 (const float4* n, float4* p, float4* q); + void btPlaneSpace1 (const float4* n, float4* p, float4* q) +{ + if (fabs(n[0].z) > 0.70710678f) { + // choose p in y-z plane + float a = n[0].y*n[0].y + n[0].z*n[0].z; + float k = 1.f/sqrt(a); + p[0].x = 0; + p[0].y = -n[0].z*k; + p[0].z = n[0].y*k; + // set q = n x p + q[0].x = a*k; + q[0].y = -n[0].x*p[0].z; + q[0].z = n[0].x*p[0].y; + } + else { + // choose p in x-y plane + float a = n[0].x*n[0].x + n[0].y*n[0].y; + float k = 1.f/sqrt(a); + p[0].x = -n[0].y*k; + p[0].y = n[0].x*k; + p[0].z = 0; + // set q = n x p + q[0].x = -n[0].z*p[0].y; + q[0].y = n[0].z*p[0].x; + q[0].z = a*k; + } +} + +void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs); +void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs) +{ + //float frictionCoeff = ldsCs[0].m_linear.w; + int aIdx = ldsCs[0].m_bodyA; + int bIdx = ldsCs[0].m_bodyB; + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA, + posB, &linVelB, &angVelB, invMassB, invInertiaB ); + + if (gBodies[aIdx].m_invMass) + { + gBodies[aIdx].m_linVel = linVelA; + gBodies[aIdx].m_angVel = angVelA; + } else + { + gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0); + gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0); + + } + if (gBodies[bIdx].m_invMass) + { + gBodies[bIdx].m_linVel = linVelB; + gBodies[bIdx].m_angVel = angVelB; + } else + { + gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0); + gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0); + + } + +} + + + +typedef struct +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +} SolverDebugInfo; + + + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void BatchSolveKernelContact(__global Body* gBodies, + __global Shape* gShapes, + __global Constraint4* gConstraints, + __global int* gN, + __global int* gOffsets, + __global int* batchSizes, + int maxBatch1, + int cellBatch, + int4 nSplit + ) +{ + //__local int ldsBatchIdx[WG_SIZE+1]; + __local int ldsCurBatch; + __local int ldsNextBatch; + __local int ldsStart; + + int lIdx = GET_LOCAL_IDX; + int wgIdx = GET_GROUP_IDX; + +// int gIdx = GET_GLOBAL_IDX; +// debugInfo[gIdx].m_valInt0 = gIdx; + //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE; + + + + + int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2); + int remain= (wgIdx%((nSplit.x*nSplit.y)/4)); + int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1); + int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1); + int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y); + + //int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1); + //int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1); + //int cellIdx = xIdx+yIdx*nSplit; + + if( gN[cellIdx] == 0 ) + return; + + int maxBatch = batchSizes[cellIdx]; + + + const int start = gOffsets[cellIdx]; + const int end = start + gN[cellIdx]; + + + + + if( lIdx == 0 ) + { + ldsCurBatch = 0; + ldsNextBatch = 0; + ldsStart = start; + } + + + GROUP_LDS_BARRIER; + + int idx=ldsStart+lIdx; + while (ldsCurBatch < maxBatch) + { + for(; idx<end; ) + { + if (gConstraints[idx].m_batchIdx == ldsCurBatch) + { + solveContactConstraint( gBodies, gShapes, &gConstraints[idx] ); + + idx+=64; + } else + { + break; + } + } + GROUP_LDS_BARRIER; + + if( lIdx == 0 ) + { + ldsCurBatch++; + } + GROUP_LDS_BARRIER; + } + + +} + + + +__kernel void solveSingleContactKernel(__global Body* gBodies, + __global Shape* gShapes, + __global Constraint4* gConstraints, + int cellIdx, + int batchOffset, + int numConstraintsInBatch + ) +{ + + int index = get_global_id(0); + if (index < numConstraintsInBatch) + { + int idx=batchOffset+index; + solveContactConstraint( gBodies, gShapes, &gConstraints[idx] ); + } +} diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.h new file mode 100644 index 0000000000..15a049992b --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveContact.h @@ -0,0 +1,393 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl new file mode 100644 index 0000000000..1d70fbbae3 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl @@ -0,0 +1,527 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Takahiro Harada + + +//#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define mymake_float4 (float4) +//#define make_float2 (float2) +//#define make_uint4 (uint4) +//#define make_int4 (int4) +//#define make_uint2 (uint2) +//#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// + + + + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = mymake_float4(a.xyz,0.f); + float4 b1 = mymake_float4(b.xyz,0.f); + return dot(a1, b1); +} + + + + +__inline +float4 normalize3(const float4 a) +{ + float4 n = mymake_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + + + + +/////////////////////////////////////// +// Matrix3x3 +/////////////////////////////////////// + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + + + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + + + + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + + + + + + + +#define WG_SIZE 64 + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + +typedef struct +{ + Matrix3x3 m_invInertia; + Matrix3x3 m_initInvInertia; +} Shape; + +typedef struct +{ + float4 m_linear; + float4 m_worldPos[4]; + float4 m_center; + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + + float m_fJacCoeffInv[2]; + float m_fAppliedRambdaDt[2]; + + u32 m_bodyA; + u32 m_bodyB; + + int m_batchIdx; + u32 m_paddings[1]; +} Constraint4; + + + +typedef struct +{ + int m_nConstraints; + int m_start; + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_solveFriction; + int m_maxBatch; // long batch really kills the performance + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBufferBatchSolve; + +void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1); + +void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1) +{ + *linear = mymake_float4(-n.xyz,0.f); + *angular0 = -cross3(r0, n); + *angular1 = cross3(r1, n); +} + +float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 ); + +float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 ) +{ + return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1); +} + + +float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1, + float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1); + +float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1, + float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1) +{ + // linear0,1 are normlized + float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0; + float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0); + float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1; + float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1); + return -1.f/(jmj0+jmj1+jmj2+jmj3); +} +void btPlaneSpace1 (const float4* n, float4* p, float4* q); + void btPlaneSpace1 (const float4* n, float4* p, float4* q) +{ + if (fabs(n[0].z) > 0.70710678f) { + // choose p in y-z plane + float a = n[0].y*n[0].y + n[0].z*n[0].z; + float k = 1.f/sqrt(a); + p[0].x = 0; + p[0].y = -n[0].z*k; + p[0].z = n[0].y*k; + // set q = n x p + q[0].x = a*k; + q[0].y = -n[0].x*p[0].z; + q[0].z = n[0].x*p[0].y; + } + else { + // choose p in x-y plane + float a = n[0].x*n[0].x + n[0].y*n[0].y; + float k = 1.f/sqrt(a); + p[0].x = -n[0].y*k; + p[0].y = n[0].x*k; + p[0].z = 0; + // set q = n x p + q[0].x = -n[0].z*p[0].y; + q[0].y = n[0].z*p[0].x; + q[0].z = a*k; + } +} + + +void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs); +void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs) +{ + float frictionCoeff = ldsCs[0].m_linear.w; + int aIdx = ldsCs[0].m_bodyA; + int bIdx = ldsCs[0].m_bodyB; + + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + + { + 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++) + { + sum +=ldsCs[0].m_appliedRambdaDt[j]; + } + frictionCoeff = 0.7f; + for(int j=0; j<4; j++) + { + maxRambdaDt[j] = frictionCoeff*sum; + minRambdaDt[j] = -maxRambdaDt[j]; + } + + +// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA, +// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt ); + + + { + + __global Constraint4* cs = ldsCs; + + if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return; + const float4 center = cs->m_center; + + float4 n = -cs->m_linear; + + float4 tangent[2]; + btPlaneSpace1(&n,&tangent[0],&tangent[1]); + float4 angular0, angular1, linear; + float4 r0 = center - posA; + float4 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 = max2( updated, minRambdaDt[i] ); + updated = min2( updated, maxRambdaDt[i] ); + rambdaDt = updated - prevSum; + cs->m_fAppliedRambdaDt[i] = updated; + } + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + linVelA += linImp0; + angVelA += angImp0; + linVelB += linImp1; + angVelB += angImp1; + } + { // angular damping for point constraint + float4 ab = normalize3( posB - posA ); + float4 ac = normalize3( center - posA ); + if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { + float angNA = dot3F4( n, angVelA ); + float angNB = dot3F4( n, angVelB ); + + angVelA -= (angNA*0.1f)*n; + angVelB -= (angNB*0.1f)*n; + } + } + } + + + + } + + if (gBodies[aIdx].m_invMass) + { + gBodies[aIdx].m_linVel = linVelA; + gBodies[aIdx].m_angVel = angVelA; + } else + { + gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0); + gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0); + } + if (gBodies[bIdx].m_invMass) + { + gBodies[bIdx].m_linVel = linVelB; + gBodies[bIdx].m_angVel = angVelB; + } else + { + gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0); + gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0); + } + + +} + +typedef struct +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +} SolverDebugInfo; + + + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void BatchSolveKernelFriction(__global Body* gBodies, + __global Shape* gShapes, + __global Constraint4* gConstraints, + __global int* gN, + __global int* gOffsets, + __global int* batchSizes, + int maxBatch1, + int cellBatch, + int4 nSplit + ) +{ + //__local int ldsBatchIdx[WG_SIZE+1]; + __local int ldsCurBatch; + __local int ldsNextBatch; + __local int ldsStart; + + int lIdx = GET_LOCAL_IDX; + int wgIdx = GET_GROUP_IDX; + +// int gIdx = GET_GLOBAL_IDX; +// debugInfo[gIdx].m_valInt0 = gIdx; + //debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE; + + + int zIdx = (wgIdx/((nSplit.x*nSplit.y)/4))*2+((cellBatch&4)>>2); + int remain= (wgIdx%((nSplit.x*nSplit.y)/4)); + int yIdx = (remain/(nSplit.x/2))*2 + ((cellBatch&2)>>1); + int xIdx = (remain%(nSplit.x/2))*2 + (cellBatch&1); + int cellIdx = xIdx+yIdx*nSplit.x+zIdx*(nSplit.x*nSplit.y); + + + if( gN[cellIdx] == 0 ) + return; + + int maxBatch = batchSizes[cellIdx]; + + const int start = gOffsets[cellIdx]; + const int end = start + gN[cellIdx]; + + + if( lIdx == 0 ) + { + ldsCurBatch = 0; + ldsNextBatch = 0; + ldsStart = start; + } + + + GROUP_LDS_BARRIER; + + int idx=ldsStart+lIdx; + while (ldsCurBatch < maxBatch) + { + for(; idx<end; ) + { + if (gConstraints[idx].m_batchIdx == ldsCurBatch) + { + + solveFrictionConstraint( gBodies, gShapes, &gConstraints[idx] ); + + idx+=64; + } else + { + break; + } + } + GROUP_LDS_BARRIER; + if( lIdx == 0 ) + { + ldsCurBatch++; + } + GROUP_LDS_BARRIER; + } + + +} + + + + + + +__kernel void solveSingleFrictionKernel(__global Body* gBodies, + __global Shape* gShapes, + __global Constraint4* gConstraints, + int cellIdx, + int batchOffset, + int numConstraintsInBatch + ) +{ + + int index = get_global_id(0); + if (index < numConstraintsInBatch) + { + + int idx=batchOffset+index; + + solveFrictionConstraint( gBodies, gShapes, &gConstraints[idx] ); + } +}
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveFriction.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveFriction.h new file mode 100644 index 0000000000..eb58674f22 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solveFriction.h @@ -0,0 +1,421 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl new file mode 100644 index 0000000000..8e2de7b5a6 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl @@ -0,0 +1,277 @@ + +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Takahiro Harada + +#include "Bullet3Dynamics/shared/b3ConvertConstraint4.h" + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// +__inline +float fastDiv(float numerator, float denominator) +{ + return native_divide(numerator, denominator); +// return numerator/denominator; +} + +__inline +float4 fastDiv4(float4 numerator, float4 denominator) +{ + return native_divide(numerator, denominator); +} + +__inline +float fastSqrtf(float f2) +{ + return native_sqrt(f2); +// return sqrt(f2); +} + +__inline +float fastRSqrt(float f2) +{ + return native_rsqrt(f2); +} + +__inline +float fastLength4(float4 v) +{ + return fast_length(v); +} + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + +__inline +float sqrtf(float a) +{ +// return sqrt(a); + return native_sqrt(a); +} + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + +__inline +float length3(const float4 a) +{ + return sqrtf(dot3F4(a,a)); +} + +__inline +float dot4(const float4 a, const float4 b) +{ + return dot( a, b ); +} + +// for height +__inline +float dot3w1(const float4 point, const float4 eqn) +{ + return dot3F4(point,eqn) + eqn.w; +} + +__inline +float4 normalize3(const float4 a) +{ + float4 n = make_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + +__inline +float4 normalize4(const float4 a) +{ + float length = sqrtf(dot4(a, a)); + return 1.f/length * a; +} + +__inline +float4 createEquation(const float4 a, const float4 b, const float4 c) +{ + float4 eqn; + float4 ab = b-a; + float4 ac = c-a; + eqn = normalize3( cross3(ab, ac) ); + eqn.w = -dot3F4(eqn,a); + return eqn; +} + + + +#define WG_SIZE 64 + + + + + + + +typedef struct +{ + int m_nConstraints; + int m_start; + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_solveFriction; + int m_maxBatch; // long batch really kills the performance + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBufferBatchSolve; + + + + + + + +typedef struct +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +} SolverDebugInfo; + + + + + + +typedef struct +{ + int m_nContacts; + float m_dt; + float m_positionDrift; + float m_positionConstraintCoeff; +} ConstBufferCTC; + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void ContactToConstraintKernel(__global struct b3Contact4Data* gContact, __global b3RigidBodyData_t* gBodies, __global b3InertiaData_t* gShapes, __global b3ContactConstraint4_t* gConstraintOut, +int nContacts, +float dt, +float positionDrift, +float positionConstraintCoeff +) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); + int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + b3Mat3x3 invInertiaA = gShapes[aIdx].m_initInvInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + b3Mat3x3 invInertiaB = gShapes[bIdx].m_initInvInertia; + + b3ContactConstraint4_t cs; + + setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB, + &gContact[gIdx], dt, positionDrift, positionConstraintCoeff, + &cs ); + + cs.m_batchIdx = gContact[gIdx].m_batchIdx; + + gConstraintOut[gIdx] = cs; + } +} + + + + + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h new file mode 100644 index 0000000000..eb1834ee00 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h @@ -0,0 +1,703 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl new file mode 100644 index 0000000000..3dc48d4350 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl @@ -0,0 +1,613 @@ +/* +Copyright (c) 2012 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Takahiro Harada + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// +__inline +float fastDiv(float numerator, float denominator) +{ + return native_divide(numerator, denominator); +// return numerator/denominator; +} + +__inline +float4 fastDiv4(float4 numerator, float4 denominator) +{ + return native_divide(numerator, denominator); +} + +__inline +float fastSqrtf(float f2) +{ + return native_sqrt(f2); +// return sqrt(f2); +} + +__inline +float fastRSqrt(float f2) +{ + return native_rsqrt(f2); +} + +__inline +float fastLength4(float4 v) +{ + return fast_length(v); +} + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + +__inline +float sqrtf(float a) +{ +// return sqrt(a); + return native_sqrt(a); +} + +__inline +float4 cross3(float4 a, float4 b) +{ + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + +__inline +float length3(const float4 a) +{ + return sqrtf(dot3F4(a,a)); +} + +__inline +float dot4(const float4 a, const float4 b) +{ + return dot( a, b ); +} + +// for height +__inline +float dot3w1(const float4 point, const float4 eqn) +{ + return dot3F4(point,eqn) + eqn.w; +} + +__inline +float4 normalize3(const float4 a) +{ + float4 n = make_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + +__inline +float4 normalize4(const float4 a) +{ + float length = sqrtf(dot4(a, a)); + return 1.f/length * a; +} + +__inline +float4 createEquation(const float4 a, const float4 b, const float4 c) +{ + float4 eqn; + float4 ab = b-a; + float4 ac = c-a; + eqn = normalize3( cross3(ab, ac) ); + eqn.w = -dot3F4(eqn,a); + return eqn; +} + +/////////////////////////////////////// +// Matrix3x3 +/////////////////////////////////////// + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + +__inline +Matrix3x3 mtZero(); + +__inline +Matrix3x3 mtIdentity(); + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m); + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b); + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + +__inline +Matrix3x3 mtZero() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(0.f); + m.m_row[1] = (float4)(0.f); + m.m_row[2] = (float4)(0.f); + return m; +} + +__inline +Matrix3x3 mtIdentity() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(1,0,0,0); + m.m_row[1] = (float4)(0,1,0,0); + m.m_row[2] = (float4)(0,0,1,0); + return m; +} + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m) +{ + Matrix3x3 out; + out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f); + out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f); + out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f); + return out; +} + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b) +{ + Matrix3x3 transB; + transB = mtTranspose( b ); + Matrix3x3 ans; + // why this doesn't run when 0ing in the for{} + a.m_row[0].w = 0.f; + a.m_row[1].w = 0.f; + a.m_row[2].w = 0.f; + for(int i=0; i<3; i++) + { +// a.m_row[i].w = 0.f; + ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]); + ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]); + ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]); + ans.m_row[i].w = 0.f; + } + return ans; +} + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + + + + +#define WG_SIZE 64 + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + +typedef struct +{ + Matrix3x3 m_invInertia; + Matrix3x3 m_initInvInertia; +} Shape; + +typedef struct +{ + float4 m_linear; + float4 m_worldPos[4]; + float4 m_center; + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + + float m_fJacCoeffInv[2]; + float m_fAppliedRambdaDt[2]; + + u32 m_bodyA; + u32 m_bodyB; + + int m_batchIdx; + u32 m_paddings[1]; +} Constraint4; + + + +typedef struct +{ + int m_nConstraints; + int m_start; + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBuffer; + +typedef struct +{ + int m_solveFriction; + int m_maxBatch; // long batch really kills the performance + int m_batchIdx; + int m_nSplit; +// int m_paddings[1]; +} ConstBufferBatchSolve; + + + + + +typedef struct +{ + int m_valInt0; + int m_valInt1; + int m_valInt2; + int m_valInt3; + + float m_val0; + float m_val1; + float m_val2; + float m_val3; +} SolverDebugInfo; + + + + +// others +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void ReorderContactKernel(__global struct b3Contact4Data* in, __global struct b3Contact4Data* out, __global int2* sortData, int4 cb ) +{ + int nContacts = cb.x; + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int srcIdx = sortData[gIdx].y; + out[gIdx] = in[srcIdx]; + } +} + +__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetDeterminismSortDataChildShapeB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataOut, int nContacts) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int2 sd; + sd.x = contactsIn[gIdx].m_childIndexB; + sd.y = gIdx; + sortDataOut[gIdx] = sd; + } +} + +__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetDeterminismSortDataChildShapeA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int2 sdIn; + sdIn = sortDataInOut[gIdx]; + int2 sdOut; + sdOut.x = contactsIn[sdIn.y].m_childIndexA; + sdOut.y = sdIn.y; + sortDataInOut[gIdx] = sdOut; + } +} + +__kernel __attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetDeterminismSortDataBodyA(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int2 sdIn; + sdIn = sortDataInOut[gIdx]; + int2 sdOut; + sdOut.x = contactsIn[sdIn.y].m_bodyAPtrAndSignBit; + sdOut.y = sdIn.y; + sortDataInOut[gIdx] = sdOut; + } +} + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetDeterminismSortDataBodyB(__global struct b3Contact4Data* contactsIn, __global int2* sortDataInOut, int nContacts) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int2 sdIn; + sdIn = sortDataInOut[gIdx]; + int2 sdOut; + sdOut.x = contactsIn[sdIn.y].m_bodyBPtrAndSignBit; + sdOut.y = sdIn.y; + sortDataInOut[gIdx] = sdOut; + } +} + + + + +typedef struct +{ + int m_nContacts; + int m_staticIdx; + float m_scale; + int m_nSplit; +} ConstBufferSSD; + + +__constant const int gridTable4x4[] = +{ + 0,1,17,16, + 1,2,18,19, + 17,18,32,3, + 16,19,3,34 +}; + +__constant 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 + +}; + + + + +#define USE_SPATIAL_BATCHING 1 +#define USE_4x4_GRID 1 + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void SetSortDataKernel(__global struct b3Contact4Data* gContact, __global Body* gBodies, __global int2* gSortDataOut, +int nContacts,float scale,int4 nSplit,int staticIdx) + +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int aPtrAndSignBit = gContact[gIdx].m_bodyAPtrAndSignBit; + int bPtrAndSignBit = gContact[gIdx].m_bodyBPtrAndSignBit; + + int aIdx = abs(aPtrAndSignBit ); + int bIdx = abs(bPtrAndSignBit); + + bool aStatic = (aPtrAndSignBit<0) ||(aPtrAndSignBit==staticIdx); + bool bStatic = (bPtrAndSignBit<0) ||(bPtrAndSignBit==staticIdx); + +#if USE_SPATIAL_BATCHING + int idx = (aStatic)? bIdx: aIdx; + float4 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 + #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 newIndex = gridTable4x4[gridIndex]; + #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 newIndex = gridTable8x8[gridIndex]; + #endif//USE_4x4_GRID +#endif//USE_SPATIAL_BATCHING + + + gSortDataOut[gIdx].x = newIndex; + gSortDataOut[gIdx].y = gIdx; + } + else + { + gSortDataOut[gIdx].x = 0xffffffff; + } +} + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void CopyConstraintKernel(__global struct b3Contact4Data* gIn, __global struct b3Contact4Data* gOut, int4 cb ) +{ + int gIdx = GET_GLOBAL_IDX; + if( gIdx < cb.x ) + { + gOut[gIdx] = gIn[gIdx]; + } +} + + + diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h new file mode 100644 index 0000000000..1b5819f6cf --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h @@ -0,0 +1,601 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl new file mode 100644 index 0000000000..a21a08c3b4 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl @@ -0,0 +1,968 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" + +#pragma OPENCL EXTENSION cl_amd_printf : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable +#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable +#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable + + +#ifdef cl_ext_atomic_counters_32 +#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable +#else +#define counter32_t volatile global int* +#endif + +typedef unsigned int u32; +typedef unsigned short u16; +typedef unsigned char u8; + +#define GET_GROUP_IDX get_group_id(0) +#define GET_LOCAL_IDX get_local_id(0) +#define GET_GLOBAL_IDX get_global_id(0) +#define GET_GROUP_SIZE get_local_size(0) +#define GET_NUM_GROUPS get_num_groups(0) +#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE) +#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE) +#define AtomInc(x) atom_inc(&(x)) +#define AtomInc1(x, out) out = atom_inc(&(x)) +#define AppendInc(x, out) out = atomic_inc(x) +#define AtomAdd(x, value) atom_add(&(x), value) +#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value ) +#define AtomXhg(x, value) atom_xchg ( &(x), value ) + + +#define SELECT_UINT4( b, a, condition ) select( b,a,condition ) + +#define make_float4 (float4) +#define make_float2 (float2) +#define make_uint4 (uint4) +#define make_int4 (int4) +#define make_uint2 (uint2) +#define make_int2 (int2) + + +#define max2 max +#define min2 min + + +/////////////////////////////////////// +// Vector +/////////////////////////////////////// +__inline +float fastDiv(float numerator, float denominator) +{ + return native_divide(numerator, denominator); +// return numerator/denominator; +} + +__inline +float4 fastDiv4(float4 numerator, float4 denominator) +{ + return native_divide(numerator, denominator); +} + +__inline +float fastSqrtf(float f2) +{ + return native_sqrt(f2); +// return sqrt(f2); +} + +__inline +float fastRSqrt(float f2) +{ + return native_rsqrt(f2); +} + +__inline +float fastLength4(float4 v) +{ + return fast_length(v); +} + +__inline +float4 fastNormalize4(float4 v) +{ + return fast_normalize(v); +} + + +__inline +float sqrtf(float a) +{ +// return sqrt(a); + return native_sqrt(a); +} + +__inline +float4 cross3(float4 a1, float4 b1) +{ + + float4 a=make_float4(a1.xyz,0.f); + float4 b=make_float4(b1.xyz,0.f); + //float4 a=a1; + //float4 b=b1; + return cross(a,b); +} + +__inline +float dot3F4(float4 a, float4 b) +{ + float4 a1 = make_float4(a.xyz,0.f); + float4 b1 = make_float4(b.xyz,0.f); + return dot(a1, b1); +} + +__inline +float length3(const float4 a) +{ + return sqrtf(dot3F4(a,a)); +} + +__inline +float dot4(const float4 a, const float4 b) +{ + return dot( a, b ); +} + +// for height +__inline +float dot3w1(const float4 point, const float4 eqn) +{ + return dot3F4(point,eqn) + eqn.w; +} + +__inline +float4 normalize3(const float4 a) +{ + float4 n = make_float4(a.x, a.y, a.z, 0.f); + return fastNormalize4( n ); +// float length = sqrtf(dot3F4(a, a)); +// return 1.f/length * a; +} + +__inline +float4 normalize4(const float4 a) +{ + float length = sqrtf(dot4(a, a)); + return 1.f/length * a; +} + +__inline +float4 createEquation(const float4 a, const float4 b, const float4 c) +{ + float4 eqn; + float4 ab = b-a; + float4 ac = c-a; + eqn = normalize3( cross3(ab, ac) ); + eqn.w = -dot3F4(eqn,a); + return eqn; +} + +/////////////////////////////////////// +// Matrix3x3 +/////////////////////////////////////// + +typedef struct +{ + float4 m_row[3]; +}Matrix3x3; + +__inline +Matrix3x3 mtZero(); + +__inline +Matrix3x3 mtIdentity(); + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m); + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b); + +__inline +float4 mtMul1(Matrix3x3 a, float4 b); + +__inline +float4 mtMul3(float4 a, Matrix3x3 b); + +__inline +Matrix3x3 mtZero() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(0.f); + m.m_row[1] = (float4)(0.f); + m.m_row[2] = (float4)(0.f); + return m; +} + +__inline +Matrix3x3 mtIdentity() +{ + Matrix3x3 m; + m.m_row[0] = (float4)(1,0,0,0); + m.m_row[1] = (float4)(0,1,0,0); + m.m_row[2] = (float4)(0,0,1,0); + return m; +} + +__inline +Matrix3x3 mtTranspose(Matrix3x3 m) +{ + Matrix3x3 out; + out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f); + out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f); + out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f); + return out; +} + +__inline +Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b) +{ + Matrix3x3 transB; + transB = mtTranspose( b ); + Matrix3x3 ans; + // why this doesn't run when 0ing in the for{} + a.m_row[0].w = 0.f; + a.m_row[1].w = 0.f; + a.m_row[2].w = 0.f; + for(int i=0; i<3; i++) + { +// a.m_row[i].w = 0.f; + ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]); + ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]); + ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]); + ans.m_row[i].w = 0.f; + } + return ans; +} + +__inline +float4 mtMul1(Matrix3x3 a, float4 b) +{ + float4 ans; + ans.x = dot3F4( a.m_row[0], b ); + ans.y = dot3F4( a.m_row[1], b ); + ans.z = dot3F4( a.m_row[2], b ); + ans.w = 0.f; + return ans; +} + +__inline +float4 mtMul3(float4 a, Matrix3x3 b) +{ + float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0); + float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0); + float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0); + + float4 ans; + ans.x = dot3F4( a, colx ); + ans.y = dot3F4( a, coly ); + ans.z = dot3F4( a, colz ); + return ans; +} + +/////////////////////////////////////// +// Quaternion +/////////////////////////////////////// + +typedef float4 Quaternion; + +__inline +Quaternion qtMul(Quaternion a, Quaternion b); + +__inline +Quaternion qtNormalize(Quaternion in); + +__inline +float4 qtRotate(Quaternion q, float4 vec); + +__inline +Quaternion qtInvert(Quaternion q); + + + + + +__inline +Quaternion qtMul(Quaternion a, Quaternion b) +{ + Quaternion ans; + ans = cross3( a, b ); + ans += a.w*b+b.w*a; +// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); + ans.w = a.w*b.w - dot3F4(a, b); + return ans; +} + +__inline +Quaternion qtNormalize(Quaternion in) +{ + return fastNormalize4(in); +// in /= length( in ); +// return in; +} +__inline +float4 qtRotate(Quaternion q, float4 vec) +{ + Quaternion qInv = qtInvert( q ); + float4 vcpy = vec; + vcpy.w = 0.f; + float4 out = qtMul(qtMul(q,vcpy),qInv); + return out; +} + +__inline +Quaternion qtInvert(Quaternion q) +{ + return (Quaternion)(-q.xyz, q.w); +} + +__inline +float4 qtInvRotate(const Quaternion q, float4 vec) +{ + return qtRotate( qtInvert( q ), vec ); +} + + + + +#define WG_SIZE 64 + +typedef struct +{ + float4 m_pos; + Quaternion m_quat; + float4 m_linVel; + float4 m_angVel; + + u32 m_shapeIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +} Body; + + + +typedef struct +{ + Matrix3x3 m_invInertia; + Matrix3x3 m_initInvInertia; +} Shape; + +typedef struct +{ + float4 m_linear; + float4 m_worldPos[4]; + float4 m_center; + float m_jacCoeffInv[4]; + float m_b[4]; + float m_appliedRambdaDt[4]; + + float m_fJacCoeffInv[2]; + float m_fAppliedRambdaDt[2]; + + u32 m_bodyA; + u32 m_bodyB; + int m_batchIdx; + u32 m_paddings; +} Constraint4; + + + + + + +__kernel void CountBodiesKernel(__global struct b3Contact4Data* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex) +{ + int i = GET_GLOBAL_IDX; + + if( i < numContactManifolds) + { + int pa = manifoldPtr[i].m_bodyAPtrAndSignBit; + bool isFixedA = (pa <0) || (pa == fixedBodyIndex); + int bodyIndexA = abs(pa); + if (!isFixedA) + { + AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x); + } + barrier(CLK_GLOBAL_MEM_FENCE); + int pb = manifoldPtr[i].m_bodyBPtrAndSignBit; + bool isFixedB = (pb <0) || (pb == fixedBodyIndex); + int bodyIndexB = abs(pb); + if (!isFixedB) + { + AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y); + } + } +} + +__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies) +{ + int i = GET_GLOBAL_IDX; + + if( i < numSplitBodies) + { + linearVelocities[i] = make_float4(0); + angularVelocities[i] = make_float4(0); + } +} + + +__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount, +__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies) +{ + int i = GET_GLOBAL_IDX; + if (i<numBodies) + { + if (gBodies[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + float factor = 1.f/((float)count); + float4 averageLinVel = make_float4(0.f); + float4 averageAngVel = make_float4(0.f); + + for (int j=0;j<count;j++) + { + averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor; + averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor; + } + + for (int j=0;j<count;j++) + { + deltaLinearVelocities[bodyOffset+j] = averageLinVel; + deltaAngularVelocities[bodyOffset+j] = averageAngVel; + } + + }//bodies[i].m_invMass + }//i<numBodies +} + + + +void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1) +{ + *linear = make_float4(n.xyz,0.f); + *angular0 = cross3(r0, n); + *angular1 = -cross3(r1, n); +} + + +float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 ) +{ + return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1); +} + + +float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1, + float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1, float countA, float countB) +{ + // linear0,1 are normlized + float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0; + float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0); + float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1; + float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1); + return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB); +} + + +void btPlaneSpace1 (float4 n, float4* p, float4* q); + void btPlaneSpace1 (float4 n, float4* p, float4* q) +{ + if (fabs(n.z) > 0.70710678f) { + // choose p in y-z plane + float a = n.y*n.y + n.z*n.z; + float k = 1.f/sqrt(a); + p[0].x = 0; + p[0].y = -n.z*k; + p[0].z = n.y*k; + // set q = n x p + q[0].x = a*k; + q[0].y = -n.x*p[0].z; + q[0].z = n.x*p[0].y; + } + else { + // choose p in x-y plane + float a = n.x*n.x + n.y*n.y; + float k = 1.f/sqrt(a); + p[0].x = -n.y*k; + p[0].y = n.x*k; + p[0].z = 0; + // set q = n x p + q[0].x = -n.z*p[0].y; + q[0].y = n.z*p[0].x; + q[0].z = a*k; + } +} + + + + + +void solveContact(__global Constraint4* cs, + float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA, + float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB, + float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB) +{ + float minRambdaDt = 0; + float maxRambdaDt = FLT_MAX; + + for(int ic=0; ic<4; ic++) + { + if( cs->m_jacCoeffInv[ic] == 0.f ) continue; + + float4 angular0, angular1, linear; + float4 r0 = cs->m_worldPos[ic] - posA; + float4 r1 = cs->m_worldPos[ic] - posB; + 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]; + rambdaDt *= cs->m_jacCoeffInv[ic]; + + + { + float prevSum = cs->m_appliedRambdaDt[ic]; + float updated = prevSum; + updated += rambdaDt; + updated = max2( updated, minRambdaDt ); + updated = min2( updated, maxRambdaDt ); + rambdaDt = updated - prevSum; + cs->m_appliedRambdaDt[ic] = updated; + } + + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + + if (invMassA) + { + *dLinVelA += linImp0; + *dAngVelA += angImp0; + } + if (invMassB) + { + *dLinVelB += linImp1; + *dAngVelB += angImp1; + } + } +} + + +// solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities); + + +void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, +__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies, +__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities) +{ + + //float frictionCoeff = ldsCs[0].m_linear.w; + int aIdx = ldsCs[0].m_bodyA; + int bIdx = ldsCs[0].m_bodyB; + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + + float4 dLinVelA = make_float4(0,0,0,0); + float4 dAngVelA = make_float4(0,0,0,0); + float4 dLinVelB = make_float4(0,0,0,0); + float4 dAngVelB = make_float4(0,0,0,0); + + int bodyOffsetA = offsetSplitBodies[aIdx]; + int constraintOffsetA = contactConstraintOffsets[0].x; + int splitIndexA = bodyOffsetA+constraintOffsetA; + + if (invMassA) + { + dLinVelA = deltaLinearVelocities[splitIndexA]; + dAngVelA = deltaAngularVelocities[splitIndexA]; + } + + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[0].y; + int splitIndexB= bodyOffsetB+constraintOffsetB; + + if (invMassB) + { + dLinVelB = deltaLinearVelocities[splitIndexB]; + dAngVelB = deltaAngularVelocities[splitIndexB]; + } + + solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA, + posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB); + + if (invMassA) + { + deltaLinearVelocities[splitIndexA] = dLinVelA; + deltaAngularVelocities[splitIndexA] = dAngVelA; + } + if (invMassB) + { + deltaLinearVelocities[splitIndexB] = dLinVelB; + deltaAngularVelocities[splitIndexB] = dAngVelB; + } + +} + + +__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes , +__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, +float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds +) +{ + int i = GET_GLOBAL_IDX; + if (i<numManifolds) + { + solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities); + } +} + + + + +void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, + __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies, + __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities) +{ + float frictionCoeff = 0.7f;//ldsCs[0].m_linear.w; + int aIdx = ldsCs[0].m_bodyA; + int bIdx = ldsCs[0].m_bodyB; + + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + + float4 dLinVelA = make_float4(0,0,0,0); + float4 dAngVelA = make_float4(0,0,0,0); + float4 dLinVelB = make_float4(0,0,0,0); + float4 dAngVelB = make_float4(0,0,0,0); + + int bodyOffsetA = offsetSplitBodies[aIdx]; + int constraintOffsetA = contactConstraintOffsets[0].x; + int splitIndexA = bodyOffsetA+constraintOffsetA; + + if (invMassA) + { + dLinVelA = deltaLinearVelocities[splitIndexA]; + dAngVelA = deltaAngularVelocities[splitIndexA]; + } + + int bodyOffsetB = offsetSplitBodies[bIdx]; + int constraintOffsetB = contactConstraintOffsets[0].y; + int splitIndexB= bodyOffsetB+constraintOffsetB; + + if (invMassB) + { + dLinVelB = deltaLinearVelocities[splitIndexB]; + dAngVelB = deltaAngularVelocities[splitIndexB]; + } + + + + + { + 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++) + { + sum +=ldsCs[0].m_appliedRambdaDt[j]; + } + frictionCoeff = 0.7f; + for(int j=0; j<4; j++) + { + maxRambdaDt[j] = frictionCoeff*sum; + minRambdaDt[j] = -maxRambdaDt[j]; + } + + +// solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA, +// posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt ); + + + { + + __global Constraint4* cs = ldsCs; + + if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return; + const float4 center = cs->m_center; + + float4 n = -cs->m_linear; + + float4 tangent[2]; + btPlaneSpace1(n,&tangent[0],&tangent[1]); + float4 angular0, angular1, linear; + float4 r0 = center - posA; + float4 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+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB ); + rambdaDt *= cs->m_fJacCoeffInv[i]; + + { + float prevSum = cs->m_fAppliedRambdaDt[i]; + float updated = prevSum; + updated += rambdaDt; + updated = max2( updated, minRambdaDt[i] ); + updated = min2( updated, maxRambdaDt[i] ); + rambdaDt = updated - prevSum; + cs->m_fAppliedRambdaDt[i] = updated; + } + + float4 linImp0 = invMassA*linear*rambdaDt; + float4 linImp1 = invMassB*(-linear)*rambdaDt; + float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt; + float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt; + + dLinVelA += linImp0; + dAngVelA += angImp0; + dLinVelB += linImp1; + dAngVelB += angImp1; + } + { // angular damping for point constraint + float4 ab = normalize3( posB - posA ); + float4 ac = normalize3( center - posA ); + if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) + { + float angNA = dot3F4( n, angVelA ); + float angNB = dot3F4( n, angVelB ); + + dAngVelA -= (angNA*0.1f)*n; + dAngVelB -= (angNB*0.1f)*n; + } + } + } + + + + } + + if (invMassA) + { + deltaLinearVelocities[splitIndexA] = dLinVelA; + deltaAngularVelocities[splitIndexA] = dAngVelA; + } + if (invMassB) + { + deltaLinearVelocities[splitIndexB] = dLinVelB; + deltaAngularVelocities[splitIndexB] = dAngVelB; + } + + +} + + +__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes , + __global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies, + __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, + float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds +) +{ + int i = GET_GLOBAL_IDX; + if (i<numManifolds) + { + solveFrictionConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities); + } +} + + +__kernel void UpdateBodyVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount, + __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies) +{ + int i = GET_GLOBAL_IDX; + if (i<numBodies) + { + if (gBodies[i].m_invMass) + { + int bodyOffset = offsetSplitBodies[i]; + int count = bodyCount[i]; + if (count) + { + gBodies[i].m_linVel += deltaLinearVelocities[bodyOffset]; + gBodies[i].m_angVel += deltaAngularVelocities[bodyOffset]; + } + } + } +} + + + +void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVelA, float invMassA, const Matrix3x3 invInertiaA, + const float4 posB, const float4 linVelB, const float4 angVelB, float invMassB, const Matrix3x3 invInertiaB, + __global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,float countA, float countB, + Constraint4* 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++) + { + dstC->m_appliedRambdaDt[ic] = 0.f; + } + dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f; + + + dstC->m_linear = src->m_worldNormalOnB; + dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() ); + for(int ic=0; ic<4; ic++) + { + float4 r0 = src->m_worldPosB[ic] - posA; + float4 r1 = src->m_worldPosB[ic] - posB; + + if( ic >= src->m_worldNormalOnB.w )//npoints + { + dstC->m_jacCoeffInv[ic] = 0.f; + continue; + } + + float relVelN; + { + float4 linear, angular0, angular1; + setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1); + + dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1, + invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB); + + relVelN = calcRelVel(linear, -linear, angular0, angular1, + linVelA, angVelA, linVelB, angVelB); + + float e = 0.f;//src->getRestituitionCoeff(); + if( relVelN*relVelN < 0.004f ) e = 0.f; + + dstC->m_b[ic] = e*relVelN; + //float penetration = src->m_worldPosB[ic].w; + dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv; + dstC->m_appliedRambdaDt[ic] = 0.f; + } + } + + if( src->m_worldNormalOnB.w > 0 )//npoints + { // prepare friction + float4 center = make_float4(0.f); + for(int i=0; i<src->m_worldNormalOnB.w; i++) + center += src->m_worldPosB[i]; + center /= (float)src->m_worldNormalOnB.w; + + float4 tangent[2]; + btPlaneSpace1(-src->m_worldNormalOnB,&tangent[0],&tangent[1]); + + float4 r[2]; + r[0] = center - posA; + r[1] = center - posB; + + for(int i=0; i<2; i++) + { + float4 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); + dstC->m_fAppliedRambdaDt[i] = 0.f; + } + dstC->m_center = center; + } + + for(int i=0; i<4; i++) + { + if( i<src->m_worldNormalOnB.w ) + { + dstC->m_worldPos[i] = src->m_worldPosB[i]; + } + else + { + dstC->m_worldPos[i] = make_float4(0.f); + } + } +} + + +__kernel +__attribute__((reqd_work_group_size(WG_SIZE,1,1))) +void ContactToConstraintSplitKernel(__global const struct b3Contact4Data* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, +__global const unsigned int* bodyCount, +int nContacts, +float dt, +float positionDrift, +float positionConstraintCoeff +) +{ + int gIdx = GET_GLOBAL_IDX; + + if( gIdx < nContacts ) + { + int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit); + int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit); + + float4 posA = gBodies[aIdx].m_pos; + float4 linVelA = gBodies[aIdx].m_linVel; + float4 angVelA = gBodies[aIdx].m_angVel; + float invMassA = gBodies[aIdx].m_invMass; + Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia; + + float4 posB = gBodies[bIdx].m_pos; + float4 linVelB = gBodies[bIdx].m_linVel; + float4 angVelB = gBodies[bIdx].m_angVel; + float invMassB = gBodies[bIdx].m_invMass; + Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia; + + Constraint4 cs; + + float countA = invMassA != 0.f ? (float)bodyCount[aIdx] : 1; + float countB = invMassB != 0.f ? (float)bodyCount[bIdx] : 1; + + 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; + } +}
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h new file mode 100644 index 0000000000..c0173ad9f4 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h @@ -0,0 +1,909 @@ +//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" +; diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl new file mode 100644 index 0000000000..ba8ba735d0 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl @@ -0,0 +1,22 @@ + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h" + + +__kernel void initializeGpuAabbsFull( const int numNodes, __global b3RigidBodyData_t* gBodies,__global b3Collidable_t* collidables, __global b3Aabb_t* plocalShapeAABB, __global b3Aabb_t* pAABB) +{ + int nodeID = get_global_id(0); + if( nodeID < numNodes ) + { + b3ComputeWorldAabb(nodeID, gBodies, collidables, plocalShapeAABB,pAABB); + } +} + +__kernel void clearOverlappingPairsKernel( __global int4* pairs, int numPairs) +{ + int pairId = get_global_id(0); + if( pairId< numPairs ) + { + pairs[pairId].z = 0xffffffff; + } +}
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h new file mode 100644 index 0000000000..d70e74017a --- /dev/null +++ b/thirdparty/bullet/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h @@ -0,0 +1,483 @@ +//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" +; |