diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3OpenCL/RigidBody')
39 files changed, 0 insertions, 17623 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h deleted file mode 100644 index 89c0142ab3..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h +++ /dev/null @@ -1,17 +0,0 @@ - -#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/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp deleted file mode 100644 index a271090af4..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h deleted file mode 100644 index 1f163ba7d5..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h +++ /dev/null @@ -1,128 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp deleted file mode 100644 index 089fb1f6a6..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.cpp +++ /dev/null @@ -1,1305 +0,0 @@ - -#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/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h deleted file mode 100644 index 8281aee05d..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuJacobiContactSolver.h +++ /dev/null @@ -1,56 +0,0 @@ - -#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/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp deleted file mode 100644 index 2e4f6c1572..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp +++ /dev/null @@ -1,1013 +0,0 @@ -#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/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h deleted file mode 100644 index 21a68de343..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h +++ /dev/null @@ -1,101 +0,0 @@ -#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/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h deleted file mode 100644 index 716a5ea0fc..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h +++ /dev/null @@ -1,89 +0,0 @@ - -#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/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp deleted file mode 100644 index bd9d6bb04b..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.cpp +++ /dev/null @@ -1,1068 +0,0 @@ - -/* -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/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h deleted file mode 100644 index 00bc544f02..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h +++ /dev/null @@ -1,76 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp deleted file mode 100644 index e3d235a4fd..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.cpp +++ /dev/null @@ -1,1529 +0,0 @@ - -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/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h deleted file mode 100644 index 6ab7502af3..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuPgsContactSolver.h +++ /dev/null @@ -1,37 +0,0 @@ - -#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/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp deleted file mode 100644 index fef33ad1cd..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ /dev/null @@ -1,677 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h deleted file mode 100644 index 0e5c6fec12..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h +++ /dev/null @@ -1,70 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h deleted file mode 100644 index e0a26fda17..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h +++ /dev/null @@ -1,68 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h deleted file mode 100644 index db815d9b31..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h +++ /dev/null @@ -1,210 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h deleted file mode 100644 index 7d9eea243a..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuSolverConstraint.h +++ /dev/null @@ -1,73 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3Solver.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.cpp deleted file mode 100644 index ccf67da1a8..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.cpp +++ /dev/null @@ -1,1128 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/b3Solver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.h deleted file mode 100644 index ee63531d78..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3Solver.h +++ /dev/null @@ -1,110 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl deleted file mode 100644 index 3b891b863d..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl +++ /dev/null @@ -1,353 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h deleted file mode 100644 index 7c73c96baa..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h +++ /dev/null @@ -1,387 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl deleted file mode 100644 index ba1b66d2c3..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl +++ /dev/null @@ -1,231 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h deleted file mode 100644 index 05800656cb..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h +++ /dev/null @@ -1,290 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl deleted file mode 100644 index e22bc9bc33..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl +++ /dev/null @@ -1,32 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h deleted file mode 100644 index 6e9c53e161..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h +++ /dev/null @@ -1,432 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl deleted file mode 100644 index 7f5dabe274..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl +++ /dev/null @@ -1,877 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/jointSolver.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/jointSolver.h deleted file mode 100644 index c94b55851e..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/jointSolver.h +++ /dev/null @@ -1,720 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/solveContact.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveContact.cl deleted file mode 100644 index 5c4d62e4ec..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveContact.cl +++ /dev/null @@ -1,501 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/solveContact.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveContact.h deleted file mode 100644 index 6e14ad51fc..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveContact.h +++ /dev/null @@ -1,392 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl deleted file mode 100644 index 1d70fbbae3..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl +++ /dev/null @@ -1,527 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/solveFriction.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveFriction.h deleted file mode 100644 index 9707cdb25d..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solveFriction.h +++ /dev/null @@ -1,420 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl deleted file mode 100644 index 8e2de7b5a6..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl +++ /dev/null @@ -1,277 +0,0 @@ - -/* -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/Bullet3OpenCL/RigidBody/kernels/solverSetup.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup.h deleted file mode 100644 index d53db03181..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup.h +++ /dev/null @@ -1,702 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl deleted file mode 100644 index 3dc48d4350..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl +++ /dev/null @@ -1,613 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h deleted file mode 100644 index 1e6e3579b6..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h +++ /dev/null @@ -1,600 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl deleted file mode 100644 index a21a08c3b4..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl +++ /dev/null @@ -1,968 +0,0 @@ -/* -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/Bullet3OpenCL/RigidBody/kernels/solverUtils.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverUtils.h deleted file mode 100644 index f4d98d9941..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/solverUtils.h +++ /dev/null @@ -1,908 +0,0 @@ -//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/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl deleted file mode 100644 index ba8ba735d0..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl +++ /dev/null @@ -1,22 +0,0 @@ - - -#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/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h b/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h deleted file mode 100644 index bb949b2027..0000000000 --- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h +++ /dev/null @@ -1,482 +0,0 @@ -//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"; |