diff --git a/Demos3/BasicGpuDemo/BasicGpuDemo.cpp b/Demos3/BasicGpuDemo/BasicGpuDemo.cpp index 881dae722..1022513e1 100644 --- a/Demos3/BasicGpuDemo/BasicGpuDemo.cpp +++ b/Demos3/BasicGpuDemo/BasicGpuDemo.cpp @@ -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 diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp index 26b670c1b..597d1ee0a 100644 --- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp +++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp @@ -16,6 +16,11 @@ subject to the following restrictions: #include "OpenGLInclude.h" + +#ifndef glDrawElementsInstanced +#define glDrawElementsInstanced glDrawElementsInstancedARB +#endif + #include "GLInstancingRenderer.h" #include diff --git a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp index 6cca67ebf..34779a429 100644 --- a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp +++ b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp @@ -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;igetUserConstraintPtr(); + 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;igetUserIndex(); @@ -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;isetUserConstraintPtr(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"); }; diff --git a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h index 28d172726..0b5b2847e 100644 --- a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h +++ b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h @@ -20,7 +20,7 @@ class b3GpuDynamicsWorld : public btDynamicsWorld btAlignedObjectArray m_uniqueShapes; btAlignedObjectArray m_uniqueShapeMapping; btAlignedObjectArray m_constraints; - + btAlignedObjectArray 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); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp new file mode 100644 index 000000000..fe8a14723 --- /dev/null +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp @@ -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 + + + +#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); +} diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h new file mode 100644 index 000000000..dffd76e5d --- /dev/null +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h @@ -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
+
    +
  • 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.
    • + +
  • 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.
    • + +
  • Angulars limits have these possible ranges: + + + + + + + + + + + + + + + + + + +
    AXISMIN ANGLEMAX ANGLE
    X-PIPI
    Y-PI/2PI/2
    Z-PIPI
    +
    • +
+ +*/ +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 diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h b/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h new file mode 100644 index 000000000..f421b4cf5 --- /dev/null +++ b/src/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h @@ -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 diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp index de54235de..1712ac221 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -1137,7 +1137,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, for (j=0;jbuildJacobian(); + 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? diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp index c2dd32a75..07735ad94 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -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(); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h index 8da7bf40a..6e8447102 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h @@ -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); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h index 5ca5ed7e4..b25c6c085 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h @@ -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); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h index c4d2aa0c4..7e8503e1b 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h @@ -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; diff --git a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp index 699ca28c0..ed45621d9 100644 --- a/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp +++ b/src/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp @@ -41,7 +41,7 @@ bool gDebugSkipLoadingBinary = false; #include #define b3Assert assert -#ifdef __APPLE__ +#ifndef _WIN32 #include #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 diff --git a/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h b/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h index 23249014d..d70c30f53 100644 --- a/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h +++ b/src/Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h @@ -4,7 +4,7 @@ #include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h" -template +template 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& operator=(const b3AlignedObjectArray& src) +// b3OpenCLArray& operator=(const b3AlignedObjectArray& 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& 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& 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(); diff --git a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp index 7bae4b154..df6606273 100644 --- a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp +++ b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp @@ -87,7 +87,7 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray& 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& 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; } } diff --git a/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl b/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl index a1a2dd744..9a627c141 100644 --- a/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl +++ b/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl @@ -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=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; } } diff --git a/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h b/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h index 4a0e9e478..3d1268c19 100644 --- a/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h +++ b/src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h @@ -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=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" diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp index 29f4358ab..e00f058a3 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -458,6 +458,6 @@ int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po void b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray& rays, b3AlignedObjectArray& 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()); } diff --git a/src/Bullet3OpenCL/RigidBody/b3Solver.cpp b/src/Bullet3OpenCL/RigidBody/b3Solver.cpp index 2f4390219..89672641b 100644 --- a/src/Bullet3OpenCL/RigidBody/b3Solver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3Solver.cpp @@ -791,7 +791,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf int nContacts, const ConstraintCfg& cfg ) { b3OpenCLArray* constraintNative =0; - + contactCOut->resize(nContacts); struct CB { int m_nContacts; @@ -825,7 +825,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf } - contactCOut->resize(nContacts); + } /* diff --git a/test/OpenCL/BasicInitialize/main.cpp b/test/OpenCL/BasicInitialize/main.cpp index 5ead2bb9c..74d550215 100644 --- a/test/OpenCL/BasicInitialize/main.cpp +++ b/test/OpenCL/BasicInitialize/main.cpp @@ -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 memTester(g_cxMainContext,g_cqCommandQue,0,true); + b3OpenCLArray 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 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 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); + } + + } } diff --git a/test/OpenCL/BasicInitialize/premake4.lua b/test/OpenCL/BasicInitialize/premake4.lua index 49c63ed84..146387ae8 100644 --- a/test/OpenCL/BasicInitialize/premake4.lua +++ b/test/OpenCL/BasicInitialize/premake4.lua @@ -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", }