AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
erwincoumans 2013-06-19 12:52:12 -07:00
commit 13a4084adf
21 changed files with 1908 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Demos3/BasicGpuDemo/BasicGpuDemo.cpp
View File

@ -284,9 +284,9 @@ void BasicGpuDemo::initPhysics()
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btSphereShape(btScalar(SCALING*1.f));
btCollisionShape* colShape = new btSphereShape(btScalar(SCALING*1.f));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects

5
btgui/OpenGLWindow/GLInstancingRenderer.cpp
View File

@ -16,6 +16,11 @@ subject to the following restrictions:
#include "OpenGLInclude.h"
#ifndef glDrawElementsInstanced
#define glDrawElementsInstanced glDrawElementsInstancedARB
#endif
#include "GLInstancingRenderer.h"
#include <string.h>

155
demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp
View File

@ -8,13 +8,16 @@
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
#include "LinearMath/btQuickprof.h"
#include "Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h"
#include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
#include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
#include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h"
#include "Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
#include "Bullet3Common/b3Logging.h"
@ -25,9 +28,9 @@
#endif
//#if (BT_BULLET_VERSION >= 282)
//#define BT_USE_BODY_UPDATE_REVISION
//#endif
#if (BT_BULLET_VERSION >= 282)
#define BT_USE_BODY_UPDATE_REVISION
#endif
b3GpuDynamicsWorld::b3GpuDynamicsWorld(class b3GpuSapBroadphase* bp,class b3GpuNarrowPhase* np, class b3GpuRigidBodyPipeline* rigidBodyPipeline)
@ -37,9 +40,12 @@ m_cpuGpuSync(true),
m_bp(bp),
m_np(np),
m_rigidBodyPipeline(rigidBodyPipeline),
m_localTime(0.f)
m_localTime(0.f),
m_staticBody(0)
{
btConvexHullShape* nullShape = new btConvexHullShape();
m_staticBody = new btRigidBody(0,0,nullShape);
addRigidBody(m_staticBody,0,0);
}
b3GpuDynamicsWorld::~b3GpuDynamicsWorld()
@ -63,6 +69,49 @@ int b3GpuDynamicsWorld::stepSimulation( btScalar timeStepUnused, int maxSubStep
BT_PROFILE("stepSimulation");
{
BT_PROFILE("sync constraints CPU");
//todo: determine what data has changed, or perform copy on GPU?
for (int i=0;i<m_constraints.size();i++)
{
btTypedConstraint* constraint = m_constraints[i];
b3TypedConstraint* c = (b3TypedConstraint*) constraint->getUserConstraintPtr();
if (c)
{
switch (constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint* p2 = (btPoint2PointConstraint*) constraint;
b3Point2PointConstraint* p3 = (b3Point2PointConstraint*) c;
p3->setPivotA((const b3Vector3&)p2->getPivotInA());
p3->setPivotB((const b3Vector3&)p2->getPivotInB());
p3->m_setting.m_damping = p2->m_setting.m_damping;
p3->m_setting.m_impulseClamp = p2->m_setting.m_impulseClamp;
p3->m_setting.m_tau = p2->m_setting.m_tau;
break;
};
case D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
b3Generic6DofConstraint* dof3 = (b3Generic6DofConstraint*) c;
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
b3Transform frameInA = (b3Transform&) dof2->getFrameOffsetA();
b3Transform frameInB = (b3Transform&) dof2->getFrameOffsetB();
dof3->setFrames(frameInA,frameInB,bodiesCL);
break;
}
default:
{
}
};
}
}
}
// detect any change (very simple)
{
@ -115,6 +164,7 @@ int b3GpuDynamicsWorld::stepSimulation( btScalar timeStepUnused, int maxSubStep
if (m_cpuGpuSync)
{
BT_PROFILE("cpuGpuSync");
m_cpuGpuSync = false;
m_np->writeAllBodiesToGpu();
m_bp->writeAabbsToGpu();
@ -409,7 +459,7 @@ void b3GpuDynamicsWorld::addRigidBody(btRigidBody* body)
body->setUserIndex(bodyIndex);
m_collisionObjects.push_back(body);
//btDynamicsWorld::addCollisionObject(
m_bodyUpdateRevisions.push_back(-1);
}
}
@ -417,6 +467,11 @@ void b3GpuDynamicsWorld::removeRigidBody(btRigidBody* colObj)
{
m_cpuGpuSync = true;
btDynamicsWorld::removeCollisionObject(colObj);
m_bodyUpdateRevisions.resize(this->m_collisionObjects.size());
for (int i=0;i<m_bodyUpdateRevisions.size();i++)
{
m_bodyUpdateRevisions[i] = -1;
}
int bodyIndex = colObj->getUserIndex();
@ -440,6 +495,11 @@ void b3GpuDynamicsWorld::removeCollisionObject(btCollisionObject* colObj)
{
m_cpuGpuSync = true;
btDynamicsWorld::removeCollisionObject(colObj);
m_bodyUpdateRevisions.resize(this->m_collisionObjects.size());
for (int i=0;i<m_bodyUpdateRevisions.size();i++)
{
m_bodyUpdateRevisions[i] = -1;
}
if (getNumCollisionObjects()==0)
{
m_uniqueShapes.resize(0);
@ -511,32 +571,101 @@ void b3GpuDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
}
}
void b3GpuDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("debugDrawWorld");
btCollisionWorld::debugDrawWorld();
}
void b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies)
{
m_constraints.push_back(constraint);
constraint->setUserConstraintPtr(0);
m_constraints.push_back(constraint);
switch (constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
b3Assert(constraint->getUserConstraintId()==-1);
btPoint2PointConstraint* p = (btPoint2PointConstraint*) constraint;
int rbA = p->getRigidBodyA().getUserIndex();
int rbB = p->getRigidBodyB().getUserIndex();
btVector3 pivotInB = p->getPivotInB();
if (rbB<=0)
{
pivotInB = p->getRigidBodyA().getWorldTransform()*p->getPivotInA();
rbB = m_staticBody->getUserIndex();
}
if (rbA>=0 && rbB>=0)
{
b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)p->getPivotInB());
b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
constraint->setUserConstraintPtr(p2p);
m_rigidBodyPipeline->addConstraint(p2p);
} else
{
constraint->setUserConstraintPtr(0);
b3Error("invalid body index in addConstraint.\n");
b3Error("invalid body index in addConstraint,b3Point2PointConstraint\n");
}
break;
}
case D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
int rbA = dof2->getRigidBodyA().getUserIndex();
int rbB = dof2->getRigidBodyB().getUserIndex();
btTransform frameInA = dof2->getFrameOffsetB();
btTransform frameInB = dof2->getFrameOffsetB();
if (rbA<=0)
{
frameInA = dof2->getRigidBodyB().getWorldTransform()*dof2->getFrameOffsetB();
rbA = m_staticBody->getUserIndex();
}
if (rbB<=0)
{
frameInB = dof2->getRigidBodyA().getWorldTransform()*dof2->getFrameOffsetA();
rbB = m_staticBody->getUserIndex();
}
if (rbA>=0 && rbB>=0)
{
b3Generic6DofConstraint* dof3 = new b3Generic6DofConstraint(rbA,rbB,(b3Transform&)frameInA,(b3Transform&)frameInB,false,bodiesCL);//(();//(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
{
btVector3 limit(0,0,0);
dof2->getLinearLowerLimit(limit);
dof3->setLinearLowerLimit((const b3Vector3&)limit);
dof2->setLinearUpperLimit(limit);
dof3->setLinearUpperLimit((const b3Vector3&)limit);
dof2->setAngularLowerLimit(limit);
dof3->setAngularLowerLimit((const b3Vector3&)limit);
dof2->setAngularUpperLimit(limit);
dof3->setAngularUpperLimit((const b3Vector3&)limit);
/* for (int i=0;i<6;i++)
{
dof3->setParam(BT_CONSTRAINT_STOP_CFM,dof2->getParam(BT_CONSTRAINT_STOP_CFM,i),i);
dof3->setParam(BT_CONSTRAINT_STOP_ERP,dof2->getParam(BT_CONSTRAINT_STOP_ERP,i),i);
}
*/
dof3->setBreakingImpulseThreshold(dof2->getBreakingImpulseThreshold());
}
// p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
constraint->setUserConstraintPtr(dof3);
m_rigidBodyPipeline->addConstraint(dof3);
} else
{
b3Error("invalid body index in addConstraint, btGeneric6DofConstraint.\n");
}
// b3Generic6DofConstraint
break;
}
default:
b3Warning("Warning: b3GpuDynamicsWorld::addConstraint with unsupported constraint type\n");
};

7
demos3/BasicGpuDemo/b3GpuDynamicsWorld.h
View File

@ -20,7 +20,7 @@ class b3GpuDynamicsWorld : public btDynamicsWorld
btAlignedObjectArray<const class btCollisionShape*> m_uniqueShapes;
btAlignedObjectArray<int> m_uniqueShapeMapping;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<int> m_bodyUpdateRevisions;
class b3GpuRigidBodyPipeline* m_rigidBodyPipeline;
class b3GpuNarrowPhase* m_np;
class b3GpuSapBroadphase* m_bp;
@ -29,7 +29,8 @@ class b3GpuDynamicsWorld : public btDynamicsWorld
btVector3 m_gravity;
bool m_cpuGpuSync;
float m_localTime;
class btRigidBody* m_staticBody;//used for picking and Bullet 2.x compatibility. In Bullet 3.x all constraints have explicitly 2 bodies.
int findOrRegisterCollisionShape(const btCollisionShape* colShape);
@ -43,7 +44,7 @@ public:
virtual void synchronizeMotionStates();
void debugDrawWorld() {}
void debugDrawWorld();
void setGravity(const btVector3& gravity);

823
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp Normal file
View File

@ -0,0 +1,823 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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.
*/
/*
2007-09-09
Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#include "b3Generic6DofConstraint.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
#include "Bullet3Common/b3TransformUtil.h"
#include "Bullet3Common/b3TransformUtil.h"
#include <new>
#define D6_USE_OBSOLETE_METHOD false
#define D6_USE_FRAME_OFFSET true
b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyCL* bodies)
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0),
m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
{
calculateTransforms(bodies);
}
#define GENERIC_D6_DISABLE_WARMSTARTING 1
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
{
int i = index%3;
int j = index/3;
return mat[i][j];
}
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz);
bool matrixToEulerXYZ(const b3Matrix3x3& mat,b3Vector3& xyz)
{
// // rot = cy*cz -cy*sz sy
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
//
b3Scalar fi = btGetMatrixElem(mat,2);
if (fi < b3Scalar(1.0f))
{
if (fi > b3Scalar(-1.0f))
{
xyz[0] = b3Atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
xyz[1] = b3Asin(btGetMatrixElem(mat,2));
xyz[2] = b3Atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = -B3_HALF_PI;
xyz[2] = b3Scalar(0.0);
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = b3Atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
xyz[1] = B3_HALF_PI;
xyz[2] = 0.0;
}
return false;
}
//////////////////////////// b3RotationalLimitMotor ////////////////////////////////////
int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
{
if(m_loLimit>m_hiLimit)
{
m_currentLimit = 0;//Free from violation
return 0;
}
if (test_value < m_loLimit)
{
m_currentLimit = 1;//low limit violation
m_currentLimitError = test_value - m_loLimit;
if(m_currentLimitError>B3_PI)
m_currentLimitError-=B3_2_PI;
else if(m_currentLimitError<-B3_PI)
m_currentLimitError+=B3_2_PI;
return 1;
}
else if (test_value> m_hiLimit)
{
m_currentLimit = 2;//High limit violation
m_currentLimitError = test_value - m_hiLimit;
if(m_currentLimitError>B3_PI)
m_currentLimitError-=B3_2_PI;
else if(m_currentLimitError<-B3_PI)
m_currentLimitError+=B3_2_PI;
return 2;
};
m_currentLimit = 0;//Free from violation
return 0;
}
//////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
{
b3Scalar loLimit = m_lowerLimit[limitIndex];
b3Scalar hiLimit = m_upperLimit[limitIndex];
if(loLimit > hiLimit)
{
m_currentLimit[limitIndex] = 0;//Free from violation
m_currentLimitError[limitIndex] = b3Scalar(0.f);
return 0;
}
if (test_value < loLimit)
{
m_currentLimit[limitIndex] = 2;//low limit violation
m_currentLimitError[limitIndex] = test_value - loLimit;
return 2;
}
else if (test_value> hiLimit)
{
m_currentLimit[limitIndex] = 1;//High limit violation
m_currentLimitError[limitIndex] = test_value - hiLimit;
return 1;
};
m_currentLimit[limitIndex] = 0;//Free from violation
m_currentLimitError[limitIndex] = b3Scalar(0.f);
return 0;
}
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
void b3Generic6DofConstraint::calculateAngleInfo()
{
b3Matrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
// in euler angle mode we do not actually constrain the angular velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
// to get constrain w2-w1 along ...not
// ------ --------------------- ------
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
// d(angle[1])/dt = 0 ax[1]
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
//
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
// to prove the result for angle[0], write the expression for angle[0] from
// GetInfo1 then take the derivative. to prove this for angle[2] it is
// easier to take the euler rate expression for d(angle[2])/dt with respect
// to the components of w and set that to 0.
b3Vector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
b3Vector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
m_calculatedAxis[1] = axis2.cross(axis0);
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
m_calculatedAxis[0].normalize();
m_calculatedAxis[1].normalize();
m_calculatedAxis[2].normalize();
}
static b3Transform getCenterOfMassTransform(const b3RigidBodyCL& body)
{
b3Transform tr(body.m_quat,body.m_pos);
return tr;
}
void b3Generic6DofConstraint::calculateTransforms(const b3RigidBodyCL* bodies)
{
b3Transform transA;
b3Transform transB;
transA = getCenterOfMassTransform(bodies[m_rbA]);
transB = getCenterOfMassTransform(bodies[m_rbB]);
calculateTransforms(transA,transB,bodies);
}
void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyCL* bodies)
{
m_calculatedTransformA = transA * m_frameInA;
m_calculatedTransformB = transB * m_frameInB;
calculateLinearInfo();
calculateAngleInfo();
if(m_useOffsetForConstraintFrame)
{ // get weight factors depending on masses
b3Scalar miA = bodies[m_rbA].m_invMass;
b3Scalar miB = bodies[m_rbB].m_invMass;
m_hasStaticBody = (miA < B3_EPSILON) || (miB < B3_EPSILON);
b3Scalar miS = miA + miB;
if(miS > b3Scalar(0.f))
{
m_factA = miB / miS;
}
else
{
m_factA = b3Scalar(0.5f);
}
m_factB = b3Scalar(1.0f) - m_factA;
}
}
bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
{
b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
angle = b3AdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
m_angularLimits[axis_index].m_currentPosition = angle;
//test limits
m_angularLimits[axis_index].testLimitValue(angle);
return m_angularLimits[axis_index].needApplyTorques();
}
void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
{
//prepare constraint
calculateTransforms(getCenterOfMassTransform(bodies[m_rbA]),getCenterOfMassTransform(bodies[m_rbB]),bodies);
info->m_numConstraintRows = 0;
info->nub = 6;
int i;
//test linear limits
for(i = 0; i < 3; i++)
{
if(m_linearLimits.needApplyForce(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
//test angular limits
for (i=0;i<3 ;i++ )
{
if(testAngularLimitMotor(i))
{
info->m_numConstraintRows++;
info->nub--;
}
}
}
}
void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
{
//pre-allocate all 6
info->m_numConstraintRows = 6;
info->nub = 0;
}
}
void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyCL* bodies)
{
b3Assert(!m_useSolveConstraintObsolete);
b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
const b3Vector3& linVelB = bodies[m_rbB].m_linVel;
const b3Vector3& angVelA = bodies[m_rbA].m_angVel;
const b3Vector3& angVelB = bodies[m_rbB].m_angVel;
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
}
}
void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyCL* bodies)
{
b3Assert(!m_useSolveConstraintObsolete);
//prepare constraint
calculateTransforms(transA,transB,bodies);
int i;
for (i=0;i<3 ;i++ )
{
testAngularLimitMotor(i);
}
if(m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
}
}
int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
{
// int row = 0;
//solve linear limits
b3RotationalLimitMotor limot;
for (int i=0;i<3 ;i++ )
{
if(m_linearLimits.needApplyForce(i))
{ // re-use rotational motor code
limot.m_bounce = b3Scalar(0.f);
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxLimitForce = b3Scalar(0.f);
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
b3Vector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
int flags = m_flags >> (i * B3_6DOF_FLAGS_AXIS_SHIFT);
limot.m_normalCFM = (flags & B3_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
limot.m_stopCFM = (flags & B3_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
limot.m_stopERP = (flags & B3_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
if(m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
int rotAllowed = 1; // rotations around orthos to current axis
if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
{
rotAllowed = 0;
}
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
}
else
{
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
}
}
}
return row;
}
int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_offset, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB)
{
b3Generic6DofConstraint * d6constraint = this;
int row = row_offset;
//solve angular limits
for (int i=0;i<3 ;i++ )
{
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
b3Vector3 axis = d6constraint->getAxis(i);
int flags = m_flags >> ((i + 3) * B3_6DOF_FLAGS_AXIS_SHIFT);
if(!(flags & B3_6DOF_FLAGS_CFM_NORM))
{
m_angularLimits[i].m_normalCFM = info->cfm[0];
}
if(!(flags & B3_6DOF_FLAGS_CFM_STOP))
{
m_angularLimits[i].m_stopCFM = info->cfm[0];
}
if(!(flags & B3_6DOF_FLAGS_ERP_STOP))
{
m_angularLimits[i].m_stopERP = info->erp;
}
row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
}
}
return row;
}
void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
{
(void)timeStep;
}
void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB,const b3RigidBodyCL* bodies)
{
m_frameInA = frameA;
m_frameInB = frameB;
calculateTransforms(bodies);
}
b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
{
return m_calculatedAxis[axis_index];
}
b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
{
return m_calculatedLinearDiff[axisIndex];
}
b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
{
return m_calculatedAxisAngleDiff[axisIndex];
}
void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyCL* bodies)
{
b3Scalar imA = bodies[m_rbA].m_invMass;
b3Scalar imB = bodies[m_rbB].m_invMass;
b3Scalar weight;
if(imB == b3Scalar(0.0))
{
weight = b3Scalar(1.0);
}
else
{
weight = imA / (imA + imB);
}
const b3Vector3& pA = m_calculatedTransformA.getOrigin();
const b3Vector3& pB = m_calculatedTransformB.getOrigin();
m_AnchorPos = pA * weight + pB * (b3Scalar(1.0) - weight);
return;
}
void b3Generic6DofConstraint::calculateLinearInfo()
{
m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
for(int i = 0; i < 3; i++)
{
m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
}
}
int b3Generic6DofConstraint::get_limit_motor_info2(
b3RotationalLimitMotor * limot,
const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational,int rotAllowed)
{
int srow = row * info->rowskip;
int powered = limot->m_enableMotor;
int limit = limot->m_currentLimit;
if (powered || limit)
{ // if the joint is powered, or has joint limits, add in the extra row
b3Scalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
b3Scalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
if (J1)
{
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
}
if (J2)
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
if((!rotational))
{
if (m_useOffsetForConstraintFrame)
{
b3Vector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
// get its projection to constraint axis
b3Vector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to constraint axis (and orthogonal to it)
b3Vector3 orthoB = relB - projB;
// same for bodyA
relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
b3Vector3 projA = ax1 * relA.dot(ax1);
b3Vector3 orthoA = relA - projA;
// get desired offset between frames A and B along constraint axis
b3Scalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
b3Vector3 totalDist = projA + ax1 * desiredOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * m_factA;
relB = orthoB - totalDist * m_factB;
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
if(m_hasStaticBody && (!rotAllowed))
{
tmpA *= m_factA;
tmpB *= m_factB;
}
int i;
for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
} else
{
b3Vector3 ltd; // Linear Torque Decoupling vector
b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
ltd = -c.cross(ax1);
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
info->m_constraintError[srow] = b3Scalar(0.f);
if (powered)
{
info->cfm[srow] = limot->m_normalCFM;
if(!limit)
{
b3Scalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
b3Scalar mot_fact = getMotorFactor( limot->m_currentPosition,
limot->m_loLimit,
limot->m_hiLimit,
tag_vel,
info->fps * limot->m_stopERP);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
}
}
if(limit)
{
b3Scalar k = info->fps * limot->m_stopERP;
if(!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
}
else
{
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = limot->m_stopCFM;
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -B3_INFINITY;
info->m_upperLimit[srow] = B3_INFINITY;
}
else
{
if (limit == 1)
{
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = B3_INFINITY;
}
else
{
info->m_lowerLimit[srow] = -B3_INFINITY;
info->m_upperLimit[srow] = 0;
}
// deal with bounce
if (limot->m_bounce > 0)
{
// calculate joint velocity
b3Scalar vel;
if (rotational)
{
vel = angVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= angVelB.dot(ax1);
}
else
{
vel = linVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= linVelB.dot(ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
if (vel < 0)
{
b3Scalar newc = -limot->m_bounce* vel;
if (newc > info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
else
{
if (vel > 0)
{
b3Scalar newc = -limot->m_bounce * vel;
if (newc < info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
{
if((axis >= 0) && (axis < 3))
{
switch(num)
{
case B3_CONSTRAINT_STOP_ERP :
m_linearLimits.m_stopERP[axis] = value;
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
case B3_CONSTRAINT_STOP_CFM :
m_linearLimits.m_stopCFM[axis] = value;
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
case B3_CONSTRAINT_CFM :
m_linearLimits.m_normalCFM[axis] = value;
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
default :
b3AssertConstrParams(0);
}
}
else if((axis >=3) && (axis < 6))
{
switch(num)
{
case B3_CONSTRAINT_STOP_ERP :
m_angularLimits[axis - 3].m_stopERP = value;
m_flags |= B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
case B3_CONSTRAINT_STOP_CFM :
m_angularLimits[axis - 3].m_stopCFM = value;
m_flags |= B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
case B3_CONSTRAINT_CFM :
m_angularLimits[axis - 3].m_normalCFM = value;
m_flags |= B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT);
break;
default :
b3AssertConstrParams(0);
}
}
else
{
b3AssertConstrParams(0);
}
}
///return the local value of parameter
b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
{
b3Scalar retVal = 0;
if((axis >= 0) && (axis < 3))
{
switch(num)
{
case B3_CONSTRAINT_STOP_ERP :
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopERP[axis];
break;
case B3_CONSTRAINT_STOP_CFM :
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_stopCFM[axis];
break;
case B3_CONSTRAINT_CFM :
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_linearLimits.m_normalCFM[axis];
break;
default :
b3AssertConstrParams(0);
}
}
else if((axis >=3) && (axis < 6))
{
switch(num)
{
case B3_CONSTRAINT_STOP_ERP :
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_ERP_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopERP;
break;
case B3_CONSTRAINT_STOP_CFM :
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_STOP << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_stopCFM;
break;
case B3_CONSTRAINT_CFM :
b3AssertConstrParams(m_flags & (B3_6DOF_FLAGS_CFM_NORM << (axis * B3_6DOF_FLAGS_AXIS_SHIFT)));
retVal = m_angularLimits[axis - 3].m_normalCFM;
break;
default :
b3AssertConstrParams(0);
}
}
else
{
b3AssertConstrParams(0);
}
return retVal;
}
void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1,const b3Vector3& axis2, const b3RigidBodyCL* bodies)
{
b3Vector3 zAxis = axis1.normalized();
b3Vector3 yAxis = axis2.normalized();
b3Vector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
b3Transform frameInW;
frameInW.setIdentity();
frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
xAxis[1], yAxis[1], zAxis[1],
xAxis[2], yAxis[2], zAxis[2]);
// now get constraint frame in local coordinate systems
m_frameInA = getCenterOfMassTransform(bodies[m_rbA]).inverse() * frameInW;
m_frameInB = getCenterOfMassTransform(bodies[m_rbB]).inverse() * frameInW;
calculateTransforms(bodies);
}

553
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h Normal file
View File

@ -0,0 +1,553 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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.
*/
/// 2009 March: b3Generic6DofConstraint refactored by Roman Ponomarev
/// Added support for generic constraint solver through getInfo1/getInfo2 methods
/*
2007-09-09
b3Generic6DofConstraint Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#ifndef B3_GENERIC_6DOF_CONSTRAINT_H
#define B3_GENERIC_6DOF_CONSTRAINT_H
#include "Bullet3Common/b3Vector3.h"
#include "b3JacobianEntry.h"
#include "b3TypedConstraint.h"
struct b3RigidBodyCL;
//! Rotation Limit structure for generic joints
class b3RotationalLimitMotor
{
public:
//! limit_parameters
//!@{
b3Scalar m_loLimit;//!< joint limit
b3Scalar m_hiLimit;//!< joint limit
b3Scalar m_targetVelocity;//!< target motor velocity
b3Scalar m_maxMotorForce;//!< max force on motor
b3Scalar m_maxLimitForce;//!< max force on limit
b3Scalar m_damping;//!< Damping.
b3Scalar m_limitSoftness;//! Relaxation factor
b3Scalar m_normalCFM;//!< Constraint force mixing factor
b3Scalar m_stopERP;//!< Error tolerance factor when joint is at limit
b3Scalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit
b3Scalar m_bounce;//!< restitution factor
bool m_enableMotor;
//!@}
//! temp_variables
//!@{
b3Scalar m_currentLimitError;//! How much is violated this limit
b3Scalar m_currentPosition; //! current value of angle
int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
b3Scalar m_accumulatedImpulse;
//!@}
b3RotationalLimitMotor()
{
m_accumulatedImpulse = 0.f;
m_targetVelocity = 0;
m_maxMotorForce = 0.1f;
m_maxLimitForce = 300.0f;
m_loLimit = 1.0f;
m_hiLimit = -1.0f;
m_normalCFM = 0.f;
m_stopERP = 0.2f;
m_stopCFM = 0.f;
m_bounce = 0.0f;
m_damping = 1.0f;
m_limitSoftness = 0.5f;
m_currentLimit = 0;
m_currentLimitError = 0;
m_enableMotor = false;
}
b3RotationalLimitMotor(const b3RotationalLimitMotor & limot)
{
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_normalCFM = limot.m_normalCFM;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
//! Is limited
bool isLimited()
{
if(m_loLimit > m_hiLimit) return false;
return true;
}
//! Need apply correction
bool needApplyTorques()
{
if(m_currentLimit == 0 && m_enableMotor == false) return false;
return true;
}
//! calculates error
/*!
calculates m_currentLimit and m_currentLimitError.
*/
int testLimitValue(b3Scalar test_value);
//! apply the correction impulses for two bodies
b3Scalar solveAngularLimits(b3Scalar timeStep,b3Vector3& axis, b3Scalar jacDiagABInv,b3RigidBodyCL * body0, b3RigidBodyCL * body1);
};
class b3TranslationalLimitMotor
{
public:
b3Vector3 m_lowerLimit;//!< the constraint lower limits
b3Vector3 m_upperLimit;//!< the constraint upper limits
b3Vector3 m_accumulatedImpulse;
//! Linear_Limit_parameters
//!@{
b3Scalar m_limitSoftness;//!< Softness for linear limit
b3Scalar m_damping;//!< Damping for linear limit
b3Scalar m_restitution;//! Bounce parameter for linear limit
b3Vector3 m_normalCFM;//!< Constraint force mixing factor
b3Vector3 m_stopERP;//!< Error tolerance factor when joint is at limit
b3Vector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit
//!@}
bool m_enableMotor[3];
b3Vector3 m_targetVelocity;//!< target motor velocity
b3Vector3 m_maxMotorForce;//!< max force on motor
b3Vector3 m_currentLimitError;//! How much is violated this limit
b3Vector3 m_currentLinearDiff;//! Current relative offset of constraint frames
int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
b3TranslationalLimitMotor()
{
m_lowerLimit.setValue(0.f,0.f,0.f);
m_upperLimit.setValue(0.f,0.f,0.f);
m_accumulatedImpulse.setValue(0.f,0.f,0.f);
m_normalCFM.setValue(0.f, 0.f, 0.f);
m_stopERP.setValue(0.2f, 0.2f, 0.2f);
m_stopCFM.setValue(0.f, 0.f, 0.f);
m_limitSoftness = 0.7f;
m_damping = b3Scalar(1.0f);
m_restitution = b3Scalar(0.5f);
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = false;
m_targetVelocity[i] = b3Scalar(0.f);
m_maxMotorForce[i] = b3Scalar(0.f);
}
}
b3TranslationalLimitMotor(const b3TranslationalLimitMotor & other )
{
m_lowerLimit = other.m_lowerLimit;
m_upperLimit = other.m_upperLimit;
m_accumulatedImpulse = other.m_accumulatedImpulse;
m_limitSoftness = other.m_limitSoftness ;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
m_normalCFM = other.m_normalCFM;
m_stopERP = other.m_stopERP;
m_stopCFM = other.m_stopCFM;
for(int i=0; i < 3; i++)
{
m_enableMotor[i] = other.m_enableMotor[i];
m_targetVelocity[i] = other.m_targetVelocity[i];
m_maxMotorForce[i] = other.m_maxMotorForce[i];
}
}
//! Test limit
/*!
- free means upper < lower,
- locked means upper == lower
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
inline bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
inline bool needApplyForce(int limitIndex)
{
if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
return true;
}
int testLimitValue(int limitIndex, b3Scalar test_value);
b3Scalar solveLinearAxis(
b3Scalar timeStep,
b3Scalar jacDiagABInv,
b3RigidBodyCL& body1,const b3Vector3 &pointInA,
b3RigidBodyCL& body2,const b3Vector3 &pointInB,
int limit_index,
const b3Vector3 & axis_normal_on_a,
const b3Vector3 & anchorPos);
};
enum b36DofFlags
{
B3_6DOF_FLAGS_CFM_NORM = 1,
B3_6DOF_FLAGS_CFM_STOP = 2,
B3_6DOF_FLAGS_ERP_STOP = 4
};
#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/*!
b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
currently this limit supports rotational motors<br>
<ul>
<li> For Linear limits, use b3Generic6DofConstraint.setLinearUpperLimit, b3Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the b3TranslationalLimitMotor structure accsesible through the b3Generic6DofConstraint.getTranslationalLimitMotor method.
At this moment translational motors are not supported. May be in the future. </li>
<li> For Angular limits, use the b3RotationalLimitMotor structure for configuring the limit.
This is accessible through b3Generic6DofConstraint.getLimitMotor method,
This brings support for limit parameters and motors. </li>
<li> Angulars limits have these possible ranges:
<table border=1 >
<tr>
<td><b>AXIS</b></td>
<td><b>MIN ANGLE</b></td>
<td><b>MAX ANGLE</b></td>
</tr><tr>
<td>X</td>
<td>-PI</td>
<td>PI</td>
</tr><tr>
<td>Y</td>
<td>-PI/2</td>
<td>PI/2</td>
</tr><tr>
<td>Z</td>
<td>-PI</td>
<td>PI</td>
</tr>
</table>
</li>
</ul>
*/
B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint
{
protected:
//! relative_frames
//!@{
b3Transform m_frameInA;//!< the constraint space w.r.t body A
b3Transform m_frameInB;//!< the constraint space w.r.t body B
//!@}
//! Jacobians
//!@{
b3JacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints
b3JacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints
//!@}
//! Linear_Limit_parameters
//!@{
b3TranslationalLimitMotor m_linearLimits;
//!@}
//! hinge_parameters
//!@{
b3RotationalLimitMotor m_angularLimits[3];
//!@}
protected:
//! temporal variables
//!@{
b3Scalar m_timeStep;
b3Transform m_calculatedTransformA;
b3Transform m_calculatedTransformB;
b3Vector3 m_calculatedAxisAngleDiff;
b3Vector3 m_calculatedAxis[3];
b3Vector3 m_calculatedLinearDiff;
b3Scalar m_factA;
b3Scalar m_factB;
bool m_hasStaticBody;
b3Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
bool m_useLinearReferenceFrameA;
bool m_useOffsetForConstraintFrame;
int m_flags;
//!@}
b3Generic6DofConstraint& operator=(b3Generic6DofConstraint& other)
{
b3Assert(0);
(void) other;
return *this;
}
int setAngularLimits(b3ConstraintInfo2 *info, int row_offset,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
int setLinearLimits(b3ConstraintInfo2 *info, int row, const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB);
// tests linear limits
void calculateLinearInfo();
//! calcs the euler angles between the two bodies.
void calculateAngleInfo();
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB ,bool useLinearReferenceFrameA,const b3RigidBodyCL* bodies);
//! Calcs global transform of the offsets
/*!
Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
\sa b3Generic6DofConstraint.getCalculatedTransformA , b3Generic6DofConstraint.getCalculatedTransformB, b3Generic6DofConstraint.calculateAngleInfo
*/
void calculateTransforms(const b3Transform& transA,const b3Transform& transB,const b3RigidBodyCL* bodies);
void calculateTransforms(const b3RigidBodyCL* bodies);
//! Gets the global transform of the offset for body A
/*!
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
*/
const b3Transform & getCalculatedTransformA() const
{
return m_calculatedTransformA;
}
//! Gets the global transform of the offset for body B
/*!
\sa b3Generic6DofConstraint.getFrameOffsetA, b3Generic6DofConstraint.getFrameOffsetB, b3Generic6DofConstraint.calculateAngleInfo.
*/
const b3Transform & getCalculatedTransformB() const
{
return m_calculatedTransformB;
}
const b3Transform & getFrameOffsetA() const
{
return m_frameInA;
}
const b3Transform & getFrameOffsetB() const
{
return m_frameInB;
}
b3Transform & getFrameOffsetA()
{
return m_frameInA;
}
b3Transform & getFrameOffsetB()
{
return m_frameInB;
}
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
virtual void getInfo2 (b3ConstraintInfo2* info,const b3RigidBodyCL* bodies);
void getInfo2NonVirtual (b3ConstraintInfo2* info,const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,const b3RigidBodyCL* bodies);
void updateRHS(b3Scalar timeStep);
//! Get the rotation axis in global coordinates
b3Vector3 getAxis(int axis_index) const;
//! Get the relative Euler angle
/*!
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
b3Scalar getAngle(int axis_index) const;
//! Get the relative position of the constraint pivot
/*!
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
b3Scalar getRelativePivotPosition(int axis_index) const;
void setFrames(const b3Transform & frameA, const b3Transform & frameB, const b3RigidBodyCL* bodies);
//! Test angular limit.
/*!
Calculates angular correction and returns true if limit needs to be corrected.
\pre b3Generic6DofConstraint::calculateTransforms() must be called previously.
*/
bool testAngularLimitMotor(int axis_index);
void setLinearLowerLimit(const b3Vector3& linearLower)
{
m_linearLimits.m_lowerLimit = linearLower;
}
void getLinearLowerLimit(b3Vector3& linearLower)
{
linearLower = m_linearLimits.m_lowerLimit;
}
void setLinearUpperLimit(const b3Vector3& linearUpper)
{
m_linearLimits.m_upperLimit = linearUpper;
}
void getLinearUpperLimit(b3Vector3& linearUpper)
{
linearUpper = m_linearLimits.m_upperLimit;
}
void setAngularLowerLimit(const b3Vector3& angularLower)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_loLimit = b3NormalizeAngle(angularLower[i]);
}
void getAngularLowerLimit(b3Vector3& angularLower)
{
for(int i = 0; i < 3; i++)
angularLower[i] = m_angularLimits[i].m_loLimit;
}
void setAngularUpperLimit(const b3Vector3& angularUpper)
{
for(int i = 0; i < 3; i++)
m_angularLimits[i].m_hiLimit = b3NormalizeAngle(angularUpper[i]);
}
void getAngularUpperLimit(b3Vector3& angularUpper)
{
for(int i = 0; i < 3; i++)
angularUpper[i] = m_angularLimits[i].m_hiLimit;
}
//! Retrieves the angular limit informacion
b3RotationalLimitMotor * getRotationalLimitMotor(int index)
{
return &m_angularLimits[index];
}
//! Retrieves the limit informacion
b3TranslationalLimitMotor * getTranslationalLimitMotor()
{
return &m_linearLimits;
}
//first 3 are linear, next 3 are angular
void setLimit(int axis, b3Scalar lo, b3Scalar hi)
{
if(axis<3)
{
m_linearLimits.m_lowerLimit[axis] = lo;
m_linearLimits.m_upperLimit[axis] = hi;
}
else
{
lo = b3NormalizeAngle(lo);
hi = b3NormalizeAngle(hi);
m_angularLimits[axis-3].m_loLimit = lo;
m_angularLimits[axis-3].m_hiLimit = hi;
}
}
//! Test limit
/*!
- free means upper < lower,
- locked means upper == lower
- limited means upper > lower
- limitIndex: first 3 are linear, next 3 are angular
*/
bool isLimited(int limitIndex)
{
if(limitIndex<3)
{
return m_linearLimits.isLimited(limitIndex);
}
return m_angularLimits[limitIndex-3].isLimited();
}
virtual void calcAnchorPos(const b3RigidBodyCL* bodies); // overridable
int get_limit_motor_info2( b3RotationalLimitMotor * limot,
const b3Transform& transA,const b3Transform& transB,const b3Vector3& linVelA,const b3Vector3& linVelB,const b3Vector3& angVelA,const b3Vector3& angVelB,
b3ConstraintInfo2 *info, int row, b3Vector3& ax1, int rotational, int rotAllowed = false);
// access for UseFrameOffset
bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
virtual void setParam(int num, b3Scalar value, int axis = -1);
///return the local value of parameter
virtual b3Scalar getParam(int num, int axis = -1) const;
void setAxis( const b3Vector3& axis1, const b3Vector3& axis2,const b3RigidBodyCL* bodies);
};
#endif //B3_GENERIC_6DOF_CONSTRAINT_H

155
src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h Normal file
View File

@ -0,0 +1,155 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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_JACOBIAN_ENTRY_H
#define B3_JACOBIAN_ENTRY_H
#include "Bullet3Common/b3Matrix3x3.h"
//notes:
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
// which makes the b3JacobianEntry memory layout 16 bytes
// if you only are interested in angular part, just feed massInvA and massInvB zero
/// Jacobian entry is an abstraction that allows to describe constraints
/// it can be used in combination with a constraint solver
/// Can be used to relate the effect of an impulse to the constraint error
B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry
{
public:
b3JacobianEntry() {};
//constraint between two different rigidbodies
b3JacobianEntry(
const b3Matrix3x3& world2A,
const b3Matrix3x3& world2B,
const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
const b3Vector3& jointAxis,
const b3Vector3& inertiaInvA,
const b3Scalar massInvA,
const b3Vector3& inertiaInvB,
const b3Scalar massInvB)
:m_linearJointAxis(jointAxis)
{
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
//angular constraint between two different rigidbodies
b3JacobianEntry(const b3Vector3& jointAxis,
const b3Matrix3x3& world2A,
const b3Matrix3x3& world2B,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
:m_linearJointAxis(b3Vector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
{
m_aJ= world2A*jointAxis;
m_bJ = world2B*-jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
//angular constraint between two different rigidbodies
b3JacobianEntry(const b3Vector3& axisInA,
const b3Vector3& axisInB,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
: m_linearJointAxis(b3Vector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
, m_aJ(axisInA)
, m_bJ(-axisInB)
{
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
//constraint on one rigidbody
b3JacobianEntry(
const b3Matrix3x3& world2A,
const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
const b3Vector3& jointAxis,
const b3Vector3& inertiaInvA,
const b3Scalar massInvA)
:m_linearJointAxis(jointAxis)
{
m_aJ= world2A*(rel_pos1.cross(jointAxis));
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = b3Vector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
b3Assert(m_Adiag > b3Scalar(0.0));
}
b3Scalar getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA) const
{
const b3JacobianEntry& jacA = *this;
b3Scalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
b3Scalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
return lin + ang;
}
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB,const b3Scalar massInvA,const b3Scalar massInvB) const
{
const b3JacobianEntry& jacA = *this;
b3Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
b3Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
b3Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
b3Vector3 lin0 = massInvA * lin ;
b3Vector3 lin1 = massInvB * lin;
b3Vector3 sum = ang0+ang1+lin0+lin1;
return sum[0]+sum[1]+sum[2];
}
b3Scalar getRelativeVelocity(const b3Vector3& linvelA,const b3Vector3& angvelA,const b3Vector3& linvelB,const b3Vector3& angvelB)
{
b3Vector3 linrel = linvelA - linvelB;
b3Vector3 angvela = angvelA * m_aJ;
b3Vector3 angvelb = angvelB * m_bJ;
linrel *= m_linearJointAxis;
angvela += angvelb;
angvela += linrel;
b3Scalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
return rel_vel2 + B3_EPSILON;
}
//private:
b3Vector3 m_linearJointAxis;
b3Vector3 m_aJ;
b3Vector3 m_bJ;
b3Vector3 m_0MinvJt;
b3Vector3 m_1MinvJt;
//Optimization: can be stored in the w/last component of one of the vectors
b3Scalar m_Adiag;
};
#endif //B3_JACOBIAN_ENTRY_H

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

@ -1137,7 +1137,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
for (j=0;j<numConstraints;j++)
{
b3TypedConstraint* constraint = constraints[j];
constraint->buildJacobian();
constraint->internalSetAppliedImpulse(0.0f);
}
}
@ -1169,7 +1169,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
}
if (constraints[i]->isEnabled())
{
constraints[i]->getInfo1(&info1);
constraints[i]->getInfo1(&info1,bodies);
} else
{
info1.m_numConstraintRows = 0;
@ -1289,6 +1289,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
}
{
//it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal
//because it gets multiplied iMJlB
b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal;
b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?

19
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
View File

@ -41,22 +41,13 @@ m_useSolveConstraintObsolete(false)
}
*/
void b3Point2PointConstraint::buildJacobian()
void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
{
///we need it for both methods
{
m_appliedImpulse = b3Scalar(0.);
}
getInfo1NonVirtual(info,bodies);
}
void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info)
{
getInfo1NonVirtual(info);
}
void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info)
void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)
{
if (m_useSolveConstraintObsolete)
{
@ -77,7 +68,7 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo
b3Transform trA;
trA.setIdentity();
trA.setOrigin(bodies[m_rbA].m_pos);
trA.setRotation(bodies[m_rbB].m_quat);
trA.setRotation(bodies[m_rbA].m_quat);
b3Transform trB;
trB.setIdentity();

5
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
View File

@ -78,11 +78,10 @@ public:
//b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA);
virtual void buildJacobian();
virtual void getInfo1 (b3ConstraintInfo1* info);
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
void getInfo1NonVirtual (b3ConstraintInfo1* info);
void getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies);
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies);

2
src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
View File

@ -247,7 +247,7 @@ B3_ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
//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)
//if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);

4
src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
View File

@ -157,8 +157,6 @@ public:
m_overrideNumSolverIterations = overideNumIterations;
}
///internal method used by the constraint solver, don't use them directly
virtual void buildJacobian() {};
///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
@ -170,7 +168,7 @@ public:
}
///internal method used by the constraint solver, don't use them directly
virtual void getInfo1 (b3ConstraintInfo1* info)=0;
virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies)=0;
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies)=0;

9
src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp
View File

@ -41,7 +41,7 @@ bool gDebugSkipLoadingBinary = false;
#include <assert.h>
#define b3Assert assert
#ifdef __APPLE__
#ifndef _WIN32
#include <sys/stat.h>
#endif
@ -752,7 +752,6 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
#else
fileUpToDate = true;
#ifdef __APPLE__
if (mkdir(sCachedBinaryPath,0777) == -1)
{
}
@ -760,7 +759,6 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
{
b3Printf("Succesfully created cache directory: %s\n", sCachedBinaryPath);
}
#endif
#endif //_WIN32
}
@ -873,10 +871,9 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
size_t program_length = kernelSource ? strlen(kernelSource) : 0;
#ifdef MAC //or __APPLE__?
char* flags = "-cl-mad-enable -DMAC -DGUID_ARG";
char* flags = "-cl-mad-enable -DMAC ";
#else
//const char* flags = "-DGUID_ARG= -fno-alias";
const char* flags = "-DGUID_ARG= ";
const char* flags = "";
#endif

34
src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h
View File

@ -4,7 +4,7 @@
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
template <typename T>
template <typename T>
class b3OpenCLArray
{
size_t m_size;
@ -59,9 +59,9 @@ public:
m_size = sizeInElements;
m_capacity = sizeInElements;
}
// we could enable this assignment, but need to make sure to avoid accidental deep copies
// b3OpenCLArray<T>& operator=(const b3AlignedObjectArray<T>& src)
// b3OpenCLArray<T>& operator=(const b3AlignedObjectArray<T>& src)
// {
// copyFromArray(src);
// return *this;
@ -73,16 +73,16 @@ public:
return m_clBuffer;
}
virtual ~b3OpenCLArray()
{
deallocate();
m_size=0;
m_capacity=0;
}
B3_FORCE_INLINE bool push_back(const T& _Val,bool waitForCompletion=true)
{
{
bool result = true;
size_t sz = size();
if( sz == capacity() )
@ -147,7 +147,7 @@ public:
}
B3_FORCE_INLINE size_t capacity() const
{
{
return m_capacity;
}
@ -164,9 +164,9 @@ public:
//create a new OpenCL buffer
size_t memSizeInBytes = sizeof(T)*_Count;
cl_mem buf = clCreateBuffer(m_clContext, CL_MEM_READ_WRITE, memSizeInBytes, NULL, &ciErrNum);
b3Assert(ciErrNum==CL_SUCCESS);
if (ciErrNum!=CL_SUCCESS)
{
b3Error("OpenCL out-of-memory\n");
_Count = 0;
result = false;
}
@ -191,7 +191,7 @@ public:
deallocate();
m_clBuffer = buf;
m_capacity = _Count;
} else
{
@ -212,14 +212,14 @@ public:
b3Assert(m_clBuffer);
b3Assert(destination);
//likely some error, destination is same as source
b3Assert(m_clBuffer != destination);
b3Assert((firstElem+numElements)<=m_size);
cl_int status = 0;
b3Assert(numElements>0);
b3Assert(numElements<=m_size);
@ -227,7 +227,7 @@ public:
size_t srcOffsetBytes = sizeof(T)*firstElem;
size_t dstOffsetInBytes = sizeof(T)*dstOffsetInElems;
status = clEnqueueCopyBuffer( m_commandQueue, m_clBuffer, destination,
status = clEnqueueCopyBuffer( m_commandQueue, m_clBuffer, destination,
srcOffsetBytes, dstOffsetInBytes, sizeof(T)*numElements, 0, 0, 0 );
b3Assert( status == CL_SUCCESS );
@ -236,7 +236,7 @@ public:
void copyFromHost(const b3AlignedObjectArray<T>& srcArray, bool waitForCompletion=true)
{
size_t newSize = srcArray.size();
bool copyOldContents = false;
resize (newSize,copyOldContents);
if (newSize)
@ -262,7 +262,7 @@ public:
b3Error("copyFromHostPointer invalid range\n");
}
}
void copyToHost(b3AlignedObjectArray<T>& destArray, bool waitForCompletion=true) const
{
@ -274,7 +274,7 @@ public:
void copyToHostPointer(T* destPtr, size_t numElem, size_t srcFirstElem=0, bool waitForCompletion=true) const
{
b3Assert(numElem+srcFirstElem <= capacity());
if(numElem+srcFirstElem <= capacity())
{
cl_int status = 0;
@ -289,7 +289,7 @@ public:
b3Error("copyToHostPointer invalid range\n");
}
}
void copyFromOpenCLArray(const b3OpenCLArray& src)
{
size_t newSize = src.size();

31
src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp
View File

@ -87,7 +87,7 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays, b3A
b3Vector3 rayTo = rays[r].m_to;
float hitFraction = hitResults[r].m_hitFraction;
int sphereHit = -1;
int hitBodyIndex= -1;
for (int b=0;b<numBodies;b++)
{
@ -95,20 +95,35 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays, b3A
const b3Vector3& pos = bodies[b].m_pos;
const b3Quaternion& orn = bodies[b].m_quat;
b3Scalar radius = 1;
if (sphere_intersect(pos, radius, rayFrom, rayTo,hitFraction))
switch (collidables[bodies[b].m_collidableIdx].m_shapeType)
{
sphereHit = b;
case SHAPE_SPHERE:
{
b3Scalar radius = collidables[bodies[b].m_collidableIdx].m_radius;
if (sphere_intersect(pos, radius, rayFrom, rayTo,hitFraction))
{
hitBodyIndex = b;
}
}
default:
{
static bool once=true;
if (once)
{
once=false;
b3Warning("Raytest: unsupported shape type\n");
}
}
}
}
if (sphereHit>=0)
if (hitBodyIndex>=0)
{
hitResults[r].m_hitFraction = hitFraction;
hitResults[r].m_hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction);
hitResults[r].m_hitNormal = (hitResults[r].m_hitPoint-bodies[sphereHit].m_pos).normalize();
hitResults[r].m_hitResult0 = sphereHit;
hitResults[r].m_hitNormal = (hitResults[r].m_hitPoint-bodies[hitBodyIndex].m_pos).normalize();
hitResults[r].m_hitResult0 = hitBodyIndex;
}
}

95
src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl
View File

@ -1,4 +1,11 @@
#define SHAPE_CONVEX_HULL 3
#define SHAPE_PLANE 4
#define SHAPE_CONCAVE_TRIMESH 5
#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6
#define SHAPE_SPHERE 7
typedef struct
{
float4 m_from;
@ -30,67 +37,103 @@ typedef struct
typedef struct Collidable
{
int m_unused1;
int m_unused2;
union {
int m_numChildShapes;
int m_bvhIndex;
};
float m_radius;
int m_shapeType;
int m_shapeIndex;
} Collidable;
bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo)
bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo, float* hitFraction)
{
// rs = ray.org - sphere.center
float4 rs = rayFrom - spherePos;
rs.w = 0.f;
float4 rayDir = (rayTo-rayFrom);
float4 rs = rayFrom - spherePos;
rs.w = 0.f;
float4 rayDir = rayTo-rayFrom;
rayDir.w = 0.f;
rayDir = normalize(rayDir);
float A = dot(rayDir,rayDir);
float B = dot(rs, rayDir);
float C = dot(rs, rs) - (radius * radius);
float D = B * B - A*C;
float B = dot(rs, rayDir);
float C = dot(rs, rs) - (radius * radius);
float D = B * B - C;
if (D > 0.0)
{
float t = (-B - sqrt(D))/A;
if (D > 0.0)
{
float t = -B - sqrt(D);
if ( (t > 0.0))// && (t < isect.t) )
if ( (t >= 0.0f) && (t < (*hitFraction)) )
{
return true;//isect.t = t;
}
*hitFraction = t;
return true;
}
}
return false;
}
float4 setInterpolate3(float4 from, float4 to, float t)
{
float s = 1.0f - t;
float4 result;
result = s * from + t * to;
result.w = 0.f;
return result;
}
__kernel void rayCastKernel(
int numRays,
const __global b3RayInfo* rays,
__global b3RayHit* hits,
__global b3RayHit* hitResults,
const int numBodies,
__global Body* bodies,
__global Collidable* collidables)
{
bool hit=false;
int i = get_global_id(0);
if (i<numRays)
{
hits[i].m_hitFraction = 1.f;
hitResults[i].m_hitFraction = 1.f;
float4 rayFrom = rays[i].m_from;
float4 rayTo = rays[i].m_to;
float hitFraction = 1.f;
int hitBodyIndex= -1;
int cachedCollidableIndex = -1;
Collidable cachedCollidable;
for (int b=0;b<numBodies;b++)
{
float4 pos = bodies[b].m_pos;
// float4 orn = bodies[b].m_quat;
if (cachedCollidableIndex !=bodies[b].m_collidableIdx)
{
cachedCollidableIndex = bodies[b].m_collidableIdx;
cachedCollidable = collidables[cachedCollidableIndex];
}
float radius = 1.f;
if (sphere_intersect(pos, radius, rayFrom, rayTo))
hit = true;
if (cachedCollidable.m_shapeType == SHAPE_SPHERE)
{
float radius = cachedCollidable.m_radius;
if (sphere_intersect(pos, radius, rayFrom, rayTo, &hitFraction))
{
hitBodyIndex = b;
}
}
}
if (hitBodyIndex>=0)
{
hitResults[i].m_hitFraction = hitFraction;
hitResults[i].m_hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);
float4 hitNormal = (float4) (hitResults[i].m_hitPoint-bodies[hitBodyIndex].m_pos);
hitResults[i].m_hitNormal = normalize(hitNormal);
hitResults[i].m_hitResult0 = hitBodyIndex;
}
if (hit)
hits[i].m_hitFraction = 0.f;
}
}

95
src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h
View File

@ -1,6 +1,13 @@
//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
static const char* rayCastKernelCL= \
"\n"
"#define SHAPE_CONVEX_HULL 3\n"
"#define SHAPE_PLANE 4\n"
"#define SHAPE_CONCAVE_TRIMESH 5\n"
"#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6\n"
"#define SHAPE_SPHERE 7\n"
"\n"
"\n"
"typedef struct\n"
"{\n"
" float4 m_from;\n"
@ -32,68 +39,104 @@ static const char* rayCastKernelCL= \
"\n"
"typedef struct Collidable\n"
"{\n"
" int m_unused1;\n"
" int m_unused2;\n"
" union {\n"
" int m_numChildShapes;\n"
" int m_bvhIndex;\n"
" };\n"
" float m_radius;\n"
" int m_shapeType;\n"
" int m_shapeIndex;\n"
"} Collidable;\n"
"\n"
"bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo)\n"
"\n"
"\n"
"bool sphere_intersect(float4 spherePos, float radius, float4 rayFrom, float4 rayTo, float* hitFraction)\n"
"{\n"
" // rs = ray.org - sphere.center\n"
" float4 rs = rayFrom - spherePos;\n"
" rs.w = 0.f;\n"
" float4 rayDir = (rayTo-rayFrom);\n"
" float4 rs = rayFrom - spherePos;\n"
" rs.w = 0.f;\n"
" float4 rayDir = rayTo-rayFrom;\n"
" rayDir.w = 0.f;\n"
" rayDir = normalize(rayDir);\n"
" float A = dot(rayDir,rayDir);\n"
" float B = dot(rs, rayDir);\n"
" float C = dot(rs, rs) - (radius * radius);\n"
" \n"
" float D = B * B - A*C;\n"
"\n"
" float B = dot(rs, rayDir);\n"
" float C = dot(rs, rs) - (radius * radius);\n"
" float D = B * B - C;\n"
" if (D > 0.0)\n"
" {\n"
" float t = (-B - sqrt(D))/A;\n"
"\n"
" if (D > 0.0)\n"
" {\n"
" float t = -B - sqrt(D);\n"
" if ( (t > 0.0))// && (t < isect.t) )\n"
" if ( (t >= 0.0f) && (t < (*hitFraction)) )\n"
" {\n"
" return true;//isect.t = t;\n"
" }\n"
" *hitFraction = t;\n"
" return true;\n"
" }\n"
" }\n"
" return false;\n"
"}\n"
"\n"
"float4 setInterpolate3(float4 from, float4 to, float t)\n"
"{\n"
" float s = 1.0f - t;\n"
" float4 result;\n"
" result = s * from + t * to;\n"
" result.w = 0.f; \n"
" return result; \n"
"}\n"
"\n"
"__kernel void rayCastKernel( \n"
" int numRays, \n"
" const __global b3RayInfo* rays, \n"
" __global b3RayHit* hits, \n"
" __global b3RayHit* hitResults, \n"
" const int numBodies, \n"
" __global Body* bodies,\n"
" __global Collidable* collidables)\n"
"{\n"
"\n"
" bool hit=false;\n"
"\n"
" int i = get_global_id(0);\n"
" if (i<numRays)\n"
" {\n"
" hits[i].m_hitFraction = 1.f;\n"
" hitResults[i].m_hitFraction = 1.f;\n"
"\n"
" float4 rayFrom = rays[i].m_from;\n"
" float4 rayTo = rays[i].m_to;\n"
" float hitFraction = 1.f;\n"
" int hitBodyIndex= -1;\n"
" \n"
" int cachedCollidableIndex = -1; \n"
" Collidable cachedCollidable;\n"
" \n"
" for (int b=0;b<numBodies;b++)\n"
" {\n"
" \n"
" float4 pos = bodies[b].m_pos;\n"
" // float4 orn = bodies[b].m_quat;\n"
" if (cachedCollidableIndex !=bodies[b].m_collidableIdx)\n"
" {\n"
" cachedCollidableIndex = bodies[b].m_collidableIdx;\n"
" cachedCollidable = collidables[cachedCollidableIndex];\n"
" }\n"
" \n"
" float radius = 1.f;\n"
" \n"
" if (sphere_intersect(pos, radius, rayFrom, rayTo))\n"
" hit = true;\n"
" if (cachedCollidable.m_shapeType == SHAPE_SPHERE)\n"
" {\n"
" float radius = cachedCollidable.m_radius;\n"
" \n"
" if (sphere_intersect(pos, radius, rayFrom, rayTo, &hitFraction))\n"
" {\n"
" hitBodyIndex = b;\n"
" }\n"
" }\n"
" }\n"
" \n"
" if (hitBodyIndex>=0)\n"
" {\n"
" hitResults[i].m_hitFraction = hitFraction;\n"
" hitResults[i].m_hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);\n"
" float4 hitNormal = (float4) (hitResults[i].m_hitPoint-bodies[hitBodyIndex].m_pos);\n"
" hitResults[i].m_hitNormal = normalize(hitNormal);\n"
" hitResults[i].m_hitResult0 = hitBodyIndex;\n"
" }\n"
" if (hit)\n"
" hits[i].m_hitFraction = 0.f;\n"
" }\n"
"}\n"
"\n"

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

@ -458,6 +458,6 @@ int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
void b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults)
{
this->m_data->m_raycaster->castRaysHost(rays,hitResults,getNumBodies(),this->m_data->m_narrowphase->getBodiesCpu(),m_data->m_narrowphase->getNumCollidablesGpu(), m_data->m_narrowphase->getCollidablesCpu());
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());
}

4
src/Bullet3OpenCL/RigidBody/b3Solver.cpp
View File

@ -791,7 +791,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf
int nContacts, const ConstraintCfg& cfg )
{
b3OpenCLArray<b3GpuConstraint4>* constraintNative =0;
contactCOut->resize(nContacts);
struct CB
{
int m_nContacts;
@ -825,7 +825,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf
}
contactCOut->resize(nContacts);
}
/*

48
test/OpenCL/BasicInitialize/main.cpp
View File

@ -4,8 +4,8 @@ Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org
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,
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.
@ -44,7 +44,7 @@ int main(int argc, char* argv[])
b3Error("test error\n");
int ciErrNum = 0;
cl_device_type deviceType = CL_DEVICE_TYPE_ALL;
const char* vendorSDK = b3OpenCLUtils::getSdkVendorName();
@ -62,9 +62,9 @@ int main(int argc, char* argv[])
b3Printf(" CL_PLATFORM_VENDOR: \t\t\t%s\n",platformInfo.m_platformVendor);
b3Printf(" CL_PLATFORM_NAME: \t\t\t%s\n",platformInfo.m_platformName);
b3Printf(" CL_PLATFORM_VERSION: \t\t\t%s\n",platformInfo.m_platformVersion);
cl_context context = b3OpenCLUtils::createContextFromPlatform(platform,deviceType,&ciErrNum);
int numDevices = b3OpenCLUtils::getNumDevices(context);
b3Printf("Num Devices = %d\n", numDevices);
for (int j=0;j<numDevices;j++)
@ -75,14 +75,14 @@ int main(int argc, char* argv[])
b3OpenCLUtils::printDeviceInfo(dev);
b3OpenCLArray<char*> memTester(g_cxMainContext,g_cqCommandQue,0,true);
b3OpenCLArray<char> memTester(g_cxMainContext,g_cqCommandQue,0,true);
int maxMem = 8192;
bool result=true;
for (size_t i=1;result;i++)
{
size_t numBytes = i*1024*1024;
result = memTester.resize(numBytes,false);
if (result)
{
printf("allocated %d MB successfully\n",i);
@ -101,7 +101,7 @@ int main(int argc, char* argv[])
///Easier method to initialize OpenCL using createContextFromType for a GPU
deviceType = CL_DEVICE_TYPE_GPU;
void* glCtx=0;
void* glDC = 0;
b3Printf("Initialize OpenCL using b3OpenCLUtils::createContextFromType for CL_DEVICE_TYPE_GPU\n");
@ -124,21 +124,25 @@ int main(int argc, char* argv[])
oclCHECKERROR(ciErrNum, CL_SUCCESS);
//normally you would create and execute kernels using this command queue
b3OpenCLArray<char*> memTester(g_cxMainContext,g_cqCommandQue,0,true);
int maxMem = 8192;
bool result=true;
for (size_t i=1;result;i++)
{
size_t numBytes = i*1024*1024;
result = memTester.resize(numBytes,false);
if (result)
{
printf("allocated %d MB successfully\n",i);
} else
{
printf("allocated %d MB failed\n", i);
}
b3OpenCLArray<char> memTester(g_cxMainContext,g_cqCommandQue,0,true);
int maxMem = 8192;
bool result=true;
for (size_t i=1;result;i++)
{
size_t numBytes = i*1024*1024;
result = memTester.resize(numBytes,false);
if (result)
{
printf("allocated %d MB successfully\n",i);
} else
{
printf("allocated %d MB failed\n", i);
}
}
}

1
test/OpenCL/BasicInitialize/premake4.lua
View File

@ -19,6 +19,7 @@ function createProject(vendor)
files {
"main.cpp",
"../../../src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp",
"../../../src/Bullet3Common/b3AlignedAllocator.cpp",
"../../../src/Bullet3OpenCL/Initialize/b3OpenCLUtils.h",
"../../../src/Bullet3Common/b3Logging.cpp",
}