Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added GPU joint solver for non-contact constraints. Only point 2 point version for now, will add some other constraints soon (changes are very local)

Browse Source
This commit is contained in:
erwin coumans
2013-07-09 10:46:47 -07:00
parent b8d5cecfe3
commit c4375a09e4
18 changed files with 2502 additions and 364 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
Demos3/GpuDemos/constraints/ConstraintsDemo.cpp
View File

@@ -1,3 +1,18 @@
/*
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 "ConstraintsDemo.h"
#include "OpenGLWindow/ShapeData.h"
@@ -37,12 +52,12 @@ void GpuConstraintsDemo::setupScene(const ConstructionInfo& ci)
index+=createDynamicsObjects(ci);
m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
m_data->m_rigidBodyPipeline->setGravity(b3Vector3(4,-10,0));
// m_data->m_rigidBodyPipeline->setGravity(b3Vector3(4,-10,0));
float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0};
//float camPos[4]={1,12.5,1.5,0};
m_instancingRenderer->setCameraTargetPosition(camPos);
m_instancingRenderer->setCameraDistance(100);
m_instancingRenderer->setCameraDistance(30);
m_instancingRenderer->updateCamera();
@@ -129,7 +144,7 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
int constraintType=0;
for (int i=0;i<ci.arraySizeZ;i++)
{
constraintType=(constraintType+1)&0x11;
//constraintType=(constraintType+1)&0x11;
for (int k=0;k<ci.arraySizeX;k++)
{
@@ -146,12 +161,13 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
colIndex = m_data->m_np->registerConvexHullShape(utilPtr);
float mass = 1.f;
if (j==0)//ci.arraySizeY-1)
if (j==0 || j==ci.arraySizeY-1)
{
//mass=0.f;
mass=0.f;
}
//b3Vector3 position((j&1)+i*2.2,1+j*2.,(j&1)+k*2.2);
b3Vector3 position((-ci.arraySizeX/2*ci.gapX)+i*ci.gapX,1+j*2.,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ);
//b3Vector3 position((-ci.arraySizeX/2*ci.gapX)+i*ci.gapX,1+j*2.,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ);
b3Vector3 position(1+j*2.,10+i*ci.gapX,(-ci.arraySizeZ/2*ci.gapZ)+k*ci.gapZ);
b3Quaternion orn(0,0,0,1);
@@ -162,7 +178,7 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false);
bool useGpu = false;
b3TypedConstraint* c = 0;
if (prevBody>=0)
@@ -170,10 +186,18 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
switch (constraintType)
{
case 0:
c = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0));
{
//c = new b3Point2PointConstraint(pid,prevBody,b3Vector3(-1.1,0,0),b3Vector3(1.1,0,0));
// c->setBreakingImpulseThreshold(14);
b3Vector3 pivotInA(-1.1,0,0);
b3Vector3 pivotInB (1.1,0,0);
int cid = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(pid,prevBody,pivotInA,pivotInB);
break;
}
case 1:
{
/*
b3Transform frameInA,frameInB;
frameInA.setIdentity();
frameInB.setIdentity();
@@ -182,11 +206,13 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB);
//c->setBreakingImpulseThreshold(37.1);
*/
break;
}
case 2:
{
/*
b3Transform frameInA,frameInB;
frameInA.setIdentity();
frameInB.setIdentity();
@@ -197,6 +223,7 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
for (int i=0;i<6;i++)
dof6->setLimit(i,0,0);
c=dof6;
*/
break;
}
default:
@@ -207,9 +234,9 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const
};
if (c)
{
m_data->m_rigidBodyPipeline->addConstraint(c);//,false);
m_data->m_rigidBodyPipeline->addConstraint(c);
}
}

2
Demos3/GpuDemos/main_opengl3core.cpp
View File

@@ -89,6 +89,8 @@ GpuDemo::CreateFunc* allDemos[]=
// ConcaveSphereScene::MyCreateFunc,
GpuBoxPlaneScene::MyCreateFunc,
GpuConvexPlaneScene::MyCreateFunc,

17
src/Bullet3Common/b3Logging.cpp
View File

@@ -1,3 +1,18 @@
/*
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 "b3Logging.h"
#include <stdio.h>
@@ -39,7 +54,7 @@ void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
}
//#define B3_MAX_DEBUG_STRING_LENGTH 2048
#define B3_MAX_DEBUG_STRING_LENGTH 8192
#define B3_MAX_DEBUG_STRING_LENGTH 32768
void b3OutputPrintfVarArgsInternal(const char *str, ...)

11
src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
View File

@@ -36,7 +36,7 @@ subject to the following restrictions:
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
b3Transform getWorldTransform(b3RigidBodyCL* rb)
static b3Transform getWorldTransform(b3RigidBodyCL* rb)
{
b3Transform newTrans;
newTrans.setOrigin(rb->m_pos);
@@ -44,24 +44,24 @@ b3Transform getWorldTransform(b3RigidBodyCL* rb)
return newTrans;
}
const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia)
static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia)
{
return inertia->m_invInertiaWorld;
}
const b3Vector3& getLinearVelocity(b3RigidBodyCL* rb)
static const b3Vector3& getLinearVelocity(b3RigidBodyCL* rb)
{
return rb->m_linVel;
}
const b3Vector3& getAngularVelocity(b3RigidBodyCL* rb)
static const b3Vector3& getAngularVelocity(b3RigidBodyCL* rb)
{
return rb->m_angVel;
}
b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos)
static b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos)
{
//we also calculate lin/ang velocity for kinematic objects
return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos);
@@ -1704,6 +1704,7 @@ void b3PgsJacobiSolver::averageVelocities()
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal)
{
B3_PROFILE("solveGroupCacheFriendlyFinish");
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int i,j;

137
src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.cpp Normal file
View File

@@ -0,0 +1,137 @@
/*
Copyright (c) 2012 Advanced Micro Devices, Inc.
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
//Originally written by Erwin Coumans
#include "b3GpuGenericConstraint.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
#include <new>
#include "Bullet3Common/b3Transform.h"
void b3GpuGenericConstraint::getInfo1 (unsigned int* info,const b3RigidBodyCL* bodies)
{
switch (m_constraintType)
{
case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:
{
*info = 3;
break;
};
default:
{
b3Assert(0);
}
};
}
void getInfo2Point2Point(b3GpuGenericConstraint* constraint, b3GpuConstraintInfo2* info, const b3RigidBodyCL* 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 b3RigidBodyCL* bodies)
{
switch (m_constraintType)
{
case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:
{
getInfo2Point2Point(this,info,bodies);
break;
};
default:
{
b3Assert(0);
}
};
}

130
src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h Normal file
View File

@@ -0,0 +1,130 @@
/*
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 b3RigidBodyCL;
enum B3_CONSTRAINT_FLAGS
{
B3_CONSTRAINT_FLAG_ENABLED=1,
};
enum b3GpuGenericConstraintType
{
B3_GPU_POINT2POINT_CONSTRAINT_TYPE=3,
// 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_FIXED_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_padding[3];
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 b3RigidBodyCL* bodies);
///internal method used by the constraint solver, don't use them directly
void getInfo2 (b3GpuConstraintInfo2* info, const b3RigidBodyCL* bodies);
};
#endif //B3_GPU_GENERIC_CONSTRAINT_H

896
src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.cpp
View File

File diff suppressed because it is too large Load Diff

63
src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.h
View File

@@ -1,23 +1,76 @@
/*
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_JACOBI_SOLVER_H
#define B3_GPU_PGS_JACOBI_SOLVER_H
#include "Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h"
struct b3Contact4;
struct b3ContactPoint;
class b3Dispatcher;
#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h"
#include "Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h"
#include "b3GpuSolverBody.h"
#include "Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h"
#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
struct b3RigidBodyCL;
struct b3InertiaCL;
#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
#include "b3GpuGenericConstraint.h"
class b3GpuPgsJacobiSolver : public b3PgsJacobiSolver
class b3GpuPgsJacobiSolver
{
protected:
int m_staticIdx;
struct b3GpuPgsJacobiSolverInternalData* m_gpuData;
protected:
b3AlignedObjectArray<b3GpuSolverBody> m_tmpSolverBodyPool;
b3ConstraintArray m_tmpSolverContactConstraintPool;
b3ConstraintArray m_tmpSolverNonContactConstraintPool;
b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
b3AlignedObjectArray<unsigned int> m_tmpConstraintSizesPool;
bool m_usePgs;
void averageVelocities();
int m_maxOverrideNumSolverIterations;
int m_numSplitImpulseRecoveries;
// int getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias);
void initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyCL* rb);
public:
b3GpuPgsJacobiSolver (cl_context ctx, cl_device_id device, cl_command_queue queue,bool usePgs);
virtual~b3GpuPgsJacobiSolver ();
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyIterations(b3OpenCLArray<b3SolverConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveGroup(b3OpenCLArray<b3RigidBodyCL>* gpuBodies,b3OpenCLArray<b3InertiaCL>* gpuInertias, int numBodies,b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
void solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyCL>* gpuBodies, b3OpenCLArray<b3InertiaCL>* gpuInertias,
int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints);
int sortConstraintByBatch3( struct b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies);
};

66
src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
View File

@@ -1,3 +1,18 @@
/*
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"
@@ -46,11 +61,13 @@ b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id devic
m_data->m_device = device;
m_data->m_queue = q;
m_data->m_solver = new b3GpuPgsJacobiSolver(ctx,device,q,true);//new b3PgsJacobiSolver(true);
m_data->m_solver = new b3PgsJacobiSolver(true);//new b3PgsJacobiSolver(true);
m_data->m_gpuSolver = new b3GpuPgsJacobiSolver(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 b3GpuJacobiSolver(ctx,device,q,config.m_maxBroadphasePairs);
#endif // TEST_OTHER_GPU_SOLVER
@@ -92,6 +109,7 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline()
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
@@ -106,9 +124,10 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline()
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);
m_data->m_joints.resize(0);
}
void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint)
@@ -121,6 +140,23 @@ void b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint)
m_data->m_joints.remove(constraint);
}
int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB)
{
b3GpuGenericConstraint c;
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 = 1e30f;
c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE;
m_data->m_cpuConstraints.push_back(c);
return 0;
}
int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* frameOrnA, const float* frameOrnB)
{
return 0;
}
void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
@@ -220,25 +256,32 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
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();
int numJoints = m_data->m_cpuConstraints.size();
if (useBullet2CpuSolver && numJoints)
{
b3AlignedObjectArray<b3RigidBodyCL> hostBodies;
gpuBodies.copyToHost(hostBodies);
b3AlignedObjectArray<b3InertiaCL> hostInertias;
gpuInertias.copyToHost(hostInertias);
// b3AlignedObjectArray<b3Contact4> hostContacts;
//gpuContacts.copyToHost(hostContacts);
{
b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0;
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);
m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints);
if (useGpu)
{
m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(),&gpuBodies,&gpuInertias,numJoints, m_data->m_gpuConstraints);
} else
{
b3AlignedObjectArray<b3RigidBodyCL> hostBodies;
gpuBodies.copyToHost(hostBodies);
b3AlignedObjectArray<b3InertiaCL> 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);
}
}
gpuBodies.copyFromHost(hostBodies);
}
if (numContacts)
@@ -387,6 +430,7 @@ void b3GpuRigidBodyPipeline::setGravity(const float* grav)
void b3GpuRigidBodyPipeline::writeAllInstancesToGpu()
{
m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU);
m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints);
}

18
src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h
View File

@@ -1,3 +1,18 @@
/*
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
@@ -40,6 +55,9 @@ public:
void setGravity(const float* grav);
void reset();
int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB);
int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* frameOrnA, const float* frameOrnB);
void addConstraint(class b3TypedConstraint* constraint);
void removeConstraint(b3TypedConstraint* constraint);

23
src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h
View File

@@ -1,3 +1,18 @@
/*
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
@@ -14,6 +29,7 @@
#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h"
#include "Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h"
struct b3GpuRigidBodyPipelineInternalData
{
@@ -26,6 +42,9 @@ struct b3GpuRigidBodyPipelineInternalData
cl_kernel m_updateAabbsKernel;
class b3PgsJacobiSolver* m_solver;
class b3GpuPgsJacobiSolver* m_gpuSolver;
class b3GpuBatchingPgsSolver* m_solver2;
class b3GpuJacobiSolver* m_solver3;
class b3GpuRaycast* m_raycaster;
@@ -37,7 +56,11 @@ struct b3GpuRigidBodyPipelineInternalData
b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU;
b3OpenCLArray<b3BroadphasePair>* m_overlappingPairsGPU;
b3OpenCLArray<b3GpuGenericConstraint>* m_gpuConstraints;
b3AlignedObjectArray<b3GpuGenericConstraint> m_cpuConstraints;
b3AlignedObjectArray<b3TypedConstraint*> m_joints;
class b3GpuNarrowPhase* m_narrowphase;
b3Vector3 m_gravity;

228
src/Bullet3OpenCL/RigidBody/b3GpuSolverBody.h Normal file
View File

@@ -0,0 +1,228 @@
/*
Copyright (c) 2013 Advanced Micro Devices, Inc.
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
//Originally written by Erwin Coumans
#ifndef B3_GPU_SOLVER_BODY_H
#define B3_GPU_SOLVER_BODY_H
class b3RigidBody;
#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

14
src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl
View File

@@ -1,3 +1,17 @@
/*
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
float4 quatMult(float4 q1, float4 q2)
{

14
src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h
View File

@@ -1,5 +1,19 @@
//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"
"\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"
"\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"
"\n"
"float4 quatMult(float4 q1, float4 q2)\n"
"{\n"

598
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl
View File

@@ -1,19 +1,84 @@
/*
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_GPU_POINT2POINT_CONSTRAINT_TYPE 3
#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_invInertia;
Matrix3x3 m_invInertiaWorld;
Matrix3x3 m_initInvInertia;
} Shape;
} BodyInertia;
typedef struct
{
@@ -23,7 +88,7 @@ typedef struct
typedef struct
{
b3Transform m_worldTransform;
// b3Transform m_worldTransformUnused;
float4 m_deltaLinearVelocity;
float4 m_deltaAngularVelocity;
float4 m_angularFactor;
@@ -41,9 +106,20 @@ typedef struct
};
int padding[3];
} b3SolverBody;
} 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
{
@@ -92,24 +168,109 @@ typedef struct
} b3BatchConstraint;
#define mymake_float4 (float4)
__inline float dot3F4(float4 a, float4 b)
typedef struct
{
float4 a1 = mymake_float4(a.xyz,0.f);
float4 b1 = mymake_float4(b.xyz,0.f);
return dot(a1, b1);
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 void internalApplyImpulse(__global b3SolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude)
__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 b3SolverBody* body1, __global b3SolverBody* body2, __global b3SolverConstraint* c)
void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c)
{
float deltaImpulse = c->m_rhs-c->m_appliedImpulse.x*c->m_cfm;
float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity);
@@ -140,7 +301,7 @@ void resolveSingleConstraintRowGeneric(__global b3SolverBody* body1, __global b3
}
__kernel
void solveJointConstraintRows(__global b3SolverBody* solverBodies,
void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,
__global b3BatchConstraint* batchConstraints,
__global b3SolverConstraint* rows,
int batchOffset,
@@ -158,4 +319,415 @@ void solveJointConstraintRows(__global b3SolverBody* solverBodies,
__global b3SolverConstraint* constraint = &rows[c->m_constraintRowOffset+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 getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, __global b3BatchConstraint* batchConstraints, 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;
batchConstraints[i].m_numConstraintRows = 3;
break;
}
default:
{
}
}
}
__kernel void initBatchConstraintsKernel(__global unsigned int* rowOffsets, __global b3BatchConstraint* batchConstraints, int numConstraints)
{
int i = get_global_id(0);
if (i>=numConstraints)
return;
batchConstraints[i].m_constraintRowOffset = rowOffsets[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]);
}
}
__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)
{
// solverBodies[i].m_linearVelocity += solverBodies[i].m_deltaLinearVelocity;
// solverBodies[i].m_angularVelocity += solverBodies[i].m_deltaAngularVelocity;
bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;
bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;
}
}
__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows,
__global unsigned int* infos,
__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;
int info1 = infos[i];
if (info1)
{
__global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[batchConstraints[i].m_constraintRowOffset];
__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;
}
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(&currentConstraintRow[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_originalContactPoint = 0;
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 = &currentConstraintRow->m_contactNormal;
info2.m_J1angularAxisFloat4 = &currentConstraintRow->m_relpos1CrossNormal;
info2.m_J2linearAxisFloat4 = 0;
info2.m_J2angularAxisFloat4 = &currentConstraintRow->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 = &currentConstraintRow->m_rhs;
currentConstraintRow->m_cfm = globalCfm;
info2.m_damping = globalDamping;
info2.cfm = &currentConstraintRow->m_cfm;
info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
info2.m_numIterations = globalNumIterations;
switch (constraint->m_constraintType)
{
case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:
{
getInfo2Point2Point(constraint,&info2,bodies);
break;
}
default:
{
}
}
///finalize the constraint setup
for ( j=0;j<info1;j++)
{
__global b3SolverConstraint* solverConstraint = &currentConstraintRow[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;
}
}
}
}

596
src/Bullet3OpenCL/RigidBody/kernels/jointSolver.h
View File

@@ -1,21 +1,86 @@
//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"
"\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"
"\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"
"\n"
"\n"
"#define B3_GPU_POINT2POINT_CONSTRAINT_TYPE 3\n"
"#define B3_INFINITY 1e30f\n"
"\n"
"#define mymake_float4 (float4)\n"
"\n"
"\n"
"__inline float dot3F4(float4 a, float4 b)\n"
"{\n"
" float4 a1 = mymake_float4(a.xyz,0.f);\n"
" float4 b1 = mymake_float4(b.xyz,0.f);\n"
" return dot(a1, b1);\n"
"}\n"
"\n"
"\n"
"typedef float4 Quaternion;\n"
"\n"
"\n"
"typedef struct\n"
"{\n"
" float4 m_row[3];\n"
"}Matrix3x3;\n"
"\n"
"__inline\n"
"float4 mtMul1(Matrix3x3 a, float4 b);\n"
"\n"
"__inline\n"
"float4 mtMul3(float4 a, Matrix3x3 b);\n"
"\n"
"\n"
"\n"
"\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"
"\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"
"\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"
"\n"
"\n"
"typedef struct\n"
"{\n"
" Matrix3x3 m_invInertia;\n"
" Matrix3x3 m_invInertiaWorld;\n"
" Matrix3x3 m_initInvInertia;\n"
"} Shape;\n"
"} BodyInertia;\n"
"\n"
"\n"
"typedef struct\n"
"{\n"
@@ -25,7 +90,7 @@ static const char* solveConstraintRowsCL= \
"\n"
"typedef struct\n"
"{\n"
" b3Transform m_worldTransform;\n"
"// b3Transform m_worldTransformUnused;\n"
" float4 m_deltaLinearVelocity;\n"
" float4 m_deltaAngularVelocity;\n"
" float4 m_angularFactor;\n"
@@ -43,9 +108,20 @@ static const char* solveConstraintRowsCL= \
" };\n"
" int padding[3];\n"
"\n"
"} b3SolverBody;\n"
"} b3GpuSolverBody;\n"
"\n"
"typedef struct\n"
"{\n"
" float4 m_pos;\n"
" Quaternion m_quat;\n"
" float4 m_linVel;\n"
" float4 m_angVel;\n"
"\n"
" unsigned int m_shapeIdx;\n"
" float m_invMass;\n"
" float m_restituitionCoeff;\n"
" float m_frictionCoeff;\n"
"} b3RigidBodyCL;\n"
"\n"
"typedef struct\n"
"{\n"
@@ -94,24 +170,109 @@ static const char* solveConstraintRowsCL= \
"\n"
"} b3BatchConstraint;\n"
"\n"
"#define mymake_float4 (float4)\n"
"\n"
"\n"
"__inline float dot3F4(float4 a, float4 b)\n"
"\n"
"\n"
"typedef struct \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"
" int m_constraintType;\n"
" int m_rbA;\n"
" int m_rbB;\n"
" float m_breakingImpulseThreshold;\n"
"\n"
" float4 m_pivotInA;\n"
" float4 m_pivotInB;\n"
" Quaternion m_relTargetAB;\n"
"\n"
" int m_flags;\n"
" int m_padding[3];\n"
"} b3GpuGenericConstraint;\n"
"\n"
"\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"
"\n"
"\n"
"\n"
"\n"
"__inline\n"
"float4 cross3(float4 a, float4 b)\n"
"{\n"
" return cross(a,b);\n"
"}\n"
"\n"
"__inline void internalApplyImpulse(__global b3SolverBody* body, float4 linearComponent, float4 angularComponent,float impulseMagnitude)\n"
"__inline\n"
"float4 fastNormalize4(float4 v)\n"
"{\n"
" v = mymake_float4(v.xyz,0.f);\n"
" return fast_normalize(v);\n"
"}\n"
"\n"
"\n"
"__inline\n"
"Quaternion qtMul(Quaternion a, Quaternion b);\n"
"\n"
"__inline\n"
"Quaternion qtNormalize(Quaternion in);\n"
"\n"
"__inline\n"
"float4 qtRotate(Quaternion q, float4 vec);\n"
"\n"
"__inline\n"
"Quaternion qtInvert(Quaternion q);\n"
"\n"
"\n"
"\n"
"\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"
"\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"
"\n"
"__inline\n"
"Quaternion qtInvert(Quaternion q)\n"
"{\n"
" return (Quaternion)(-q.xyz, q.w);\n"
"}\n"
"\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"
"\n"
"\n"
"void resolveSingleConstraintRowGeneric(__global b3SolverBody* body1, __global b3SolverBody* body2, __global b3SolverConstraint* c)\n"
"void resolveSingleConstraintRowGeneric(__global b3GpuSolverBody* body1, __global b3GpuSolverBody* body2, __global b3SolverConstraint* c)\n"
"{\n"
" float deltaImpulse = c->m_rhs-c->m_appliedImpulse.x*c->m_cfm;\n"
" float deltaVel1Dotn = dot3F4(c->m_contactNormal,body1->m_deltaLinearVelocity) + dot3F4(c->m_relpos1CrossNormal,body1->m_deltaAngularVelocity);\n"
@@ -142,7 +303,7 @@ static const char* solveConstraintRowsCL= \
"}\n"
"\n"
"__kernel\n"
"void solveJointConstraintRows(__global b3SolverBody* solverBodies,\n"
"void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,\n"
" __global b3BatchConstraint* batchConstraints,\n"
" __global b3SolverConstraint* rows,\n"
" int batchOffset,\n"
@@ -161,4 +322,415 @@ static const char* solveConstraintRowsCL= \
" 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"
"\n"
" __global b3GpuSolverBody* solverBody = &solverBodies[i];\n"
" __global b3RigidBodyCL* bodyCL = &bodiesCL[i];\n"
"\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"
"\n"
"__kernel void getInfo1Kernel(__global unsigned int* infos, __global b3GpuGenericConstraint* constraints, __global b3BatchConstraint* batchConstraints, int numConstraints)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numConstraints)\n"
" return;\n"
"\n"
" __global b3GpuGenericConstraint* constraint = &constraints[i];\n"
"\n"
" switch (constraint->m_constraintType)\n"
" {\n"
" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n"
" {\n"
" infos[i] = 3;\n"
" batchConstraints[i].m_numConstraintRows = 3;\n"
" break;\n"
" }\n"
" default:\n"
" {\n"
" }\n"
" }\n"
"}\n"
"\n"
"__kernel void initBatchConstraintsKernel(__global unsigned int* rowOffsets, __global b3BatchConstraint* batchConstraints, int numConstraints)\n"
"{\n"
" int i = get_global_id(0);\n"
" if (i>=numConstraints)\n"
" return;\n"
"\n"
" batchConstraints[i].m_constraintRowOffset = rowOffsets[i];\n"
"}\n"
"\n"
"\n"
"\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"
"\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"
" };\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"
"\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"
"\n"
" // lo and hi limits for variables (set to -/+ infinity on entry).\n"
" __global float* m_lowerLimit;\n"
" __global float* m_upperLimit;\n"
"\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"
"\n"
" //damping of the velocity\n"
" float m_damping;\n"
"} b3GpuConstraintInfo2;\n"
"\n"
"\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"
"\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"
"\n"
" float4 posB = bodies[constraint->m_rbB].m_pos;\n"
" Quaternion rotB = bodies[constraint->m_rbB].m_quat;\n"
"\n"
"\n"
"\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"
"\n"
" float4 a1 = qtRotate(rotA,constraint->m_pivotInA);\n"
"\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"
"\n"
" float k = info->fps * currERP;\n"
" int j;\n"
" float4 result = a2 + posB - a1 - posA;\n"
" float* resultPtr = &result;\n"
"\n"
" for (j=0; j<3; j++)\n"
" {\n"
" info->m_constraintError[j*info->rowskip] = k * (resultPtr[j]);\n"
" }\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"
"\n"
" if (bodies[i].m_invMass)\n"
" {\n"
"// solverBodies[i].m_linearVelocity += solverBodies[i].m_deltaLinearVelocity;\n"
"// solverBodies[i].m_angularVelocity += solverBodies[i].m_deltaAngularVelocity;\n"
" bodies[i].m_linVel += solverBodies[i].m_deltaLinearVelocity;\n"
" bodies[i].m_angVel += solverBodies[i].m_deltaAngularVelocity;\n"
" }\n"
"}\n"
"\n"
"\n"
"__kernel void getInfo2Kernel(__global b3SolverConstraint* solverConstraintRows, \n"
" __global unsigned int* infos, \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"
"\n"
" int i = get_global_id(0);\n"
" if (i>=numConstraints)\n"
" return;\n"
" \n"
" int info1 = infos[i];\n"
" \n"
" if (info1)\n"
" {\n"
" __global b3SolverConstraint* currentConstraintRow = &solverConstraintRows[batchConstraints[i].m_constraintRowOffset];\n"
" __global b3GpuGenericConstraint* constraint = &constraints[i];\n"
"\n"
" __global b3RigidBodyCL* rbA = &bodies[ constraint->m_rbA];\n"
" __global b3RigidBodyCL* rbB = &bodies[ constraint->m_rbB];\n"
"\n"
" int solverBodyIdA = constraint->m_rbA;\n"
" int solverBodyIdB = constraint->m_rbB;\n"
"\n"
" __global b3GpuSolverBody* bodyAPtr = &solverBodies[solverBodyIdA];\n"
" __global b3GpuSolverBody* bodyBPtr = &solverBodies[solverBodyIdB];\n"
"\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"
"\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"
"\n"
" int overrideNumSolverIterations = 0;//constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;\n"
"// if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)\n"
" // m_maxOverrideNumSolverIterations = overrideNumSolverIterations;\n"
"\n"
"\n"
" int j;\n"
" for ( j=0;j<info1;j++)\n"
" {\n"
"// memset(&currentConstraintRow[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"
"\n"
" currentConstraintRow[j].m_originalContactPoint = 0;\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"
"\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"
"\n"
" int rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n"
"\n"
" \n"
"\n"
"\n"
" b3GpuConstraintInfo2 info2;\n"
" info2.fps = 1.f/timeStep;\n"
" info2.erp = globalErp;\n"
" info2.m_J1linearAxisFloat4 = &currentConstraintRow->m_contactNormal;\n"
" info2.m_J1angularAxisFloat4 = &currentConstraintRow->m_relpos1CrossNormal;\n"
" info2.m_J2linearAxisFloat4 = 0;\n"
" info2.m_J2angularAxisFloat4 = &currentConstraintRow->m_relpos2CrossNormal;\n"
" info2.rowskip = sizeof(b3SolverConstraint)/sizeof(float);//check this\n"
"\n"
" ///the size of b3SolverConstraint needs be a multiple of float\n"
"// b3Assert(info2.rowskip*sizeof(float)== sizeof(b3SolverConstraint));\n"
" info2.m_constraintError = &currentConstraintRow->m_rhs;\n"
" currentConstraintRow->m_cfm = globalCfm;\n"
" info2.m_damping = globalDamping;\n"
" info2.cfm = &currentConstraintRow->m_cfm;\n"
" info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;\n"
" info2.m_upperLimit = &currentConstraintRow->m_upperLimit;\n"
" info2.m_numIterations = globalNumIterations;\n"
"\n"
" switch (constraint->m_constraintType)\n"
" {\n"
" case B3_GPU_POINT2POINT_CONSTRAINT_TYPE:\n"
" {\n"
" getInfo2Point2Point(constraint,&info2,bodies);\n"
" break;\n"
" }\n"
" default:\n"
" {\n"
" }\n"
" }\n"
"\n"
" ///finalize the constraint setup\n"
" for ( j=0;j<info1;j++)\n"
" {\n"
" __global b3SolverConstraint* solverConstraint = &currentConstraintRow[j];\n"
"\n"
" if (solverConstraint->m_upperLimit>=constraint->m_breakingImpulseThreshold)\n"
" {\n"
" solverConstraint->m_upperLimit = constraint->m_breakingImpulseThreshold;\n"
" }\n"
"\n"
" if (solverConstraint->m_lowerLimit<=-constraint->m_breakingImpulseThreshold)\n"
" {\n"
" solverConstraint->m_lowerLimit = -constraint->m_breakingImpulseThreshold;\n"
" }\n"
"\n"
"// solverConstraint->m_originalContactPoint = constraint;\n"
" \n"
" Matrix3x3 invInertiaWorldA= inertias[constraint->m_rbA].m_invInertiaWorld;\n"
" {\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"
"\n"
" float4 ftorqueAxis2 = solverConstraint->m_relpos2CrossNormal;\n"
" solverConstraint->m_angularComponentB = mtMul1(invInertiaWorldB,ftorqueAxis2);//*constraint->m_rbB.getAngularFactor();\n"
" }\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"
"\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"
"\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"
"\n"
" rel_vel = vel1Dotn+vel2Dotn;\n"
"\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"
"}\n"
;

4
src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl
View File

@@ -1,5 +1,5 @@
/*
Copyright (c) 2012 Advanced Micro Devices, Inc.
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.
@@ -11,7 +11,7 @@ subject to the following restrictions:
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
#pragma OPENCL EXTENSION cl_amd_printf : enable
#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable

2
src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h
View File

@@ -13,7 +13,7 @@ static const char* solverUtilsCL= \
"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"
"\n"
"//Originally written by Erwin Coumans\n"
"\n"
"#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"