From 01e5472f9ff19cc2aefe5451380b8c8f081ff001 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Sat, 6 Jul 2013 00:44:06 -0700 Subject: [PATCH] add b3FixedConstraint, with less memory footprint than b3Generic6DofConstraint, and easier to port to OpenCL. remove the 'obsolete' part of b3Generic6DofConstraint and b3Point2PointConstraint --- .../GpuDemos/constraints/ConstraintsDemo.cpp | 59 +++++++++- .../ConstraintSolver/b3FixedConstraint.cpp | 108 ++++++++++++++++++ .../ConstraintSolver/b3FixedConstraint.h | 35 ++++++ .../b3Generic6DofConstraint.cpp | 64 ++++------- .../b3Generic6DofConstraint.h | 5 +- .../b3Point2PointConstraint.cpp | 13 +-- .../b3Point2PointConstraint.h | 3 - .../ConstraintSolver/b3TypedConstraint.h | 1 + 8 files changed, 226 insertions(+), 62 deletions(-) create mode 100644 src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp create mode 100644 src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h diff --git a/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp b/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp index d1c069937..6f6d88e5f 100644 --- a/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp +++ b/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp @@ -16,6 +16,9 @@ #include "../rigidbody/GpuRigidBodyDemoInternalData.h" #include "../gwenUserInterface.h" #include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h" +#include "Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h" +#include "Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h" + #include "OpenGLWindow/GLPrimitiveRenderer.h" #include "Bullet3OpenCL/Raycast/b3GpuRaycast.h" #include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h" @@ -34,7 +37,7 @@ void GpuConstraintsDemo::setupScene(const ConstructionInfo& ci) index+=createDynamicsObjects(ci); m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); - + m_data->m_rigidBodyPipeline->setGravity(b3Vector3(4,-10,0)); float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0}; //float camPos[4]={1,12.5,1.5,0}; @@ -123,9 +126,10 @@ int GpuConstraintsDemo::createDynamicsObjects2(const ConstructionInfo& ci, const if (ci.m_useInstancedCollisionShapes) colIndex = m_data->m_np->registerConvexHullShape(utilPtr); + int constraintType=0; for (int i=0;im_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); + b3TypedConstraint* c = 0; + if (prevBody>=0) { - b3Point2PointConstraint* p2p = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0)); - m_data->m_rigidBodyPipeline->addConstraint(p2p);//,false); + switch (constraintType) + { + case 0: + c = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0)); + break; + case 1: + { + b3Transform frameInA,frameInB; + frameInA.setIdentity(); + frameInB.setIdentity(); + frameInA.setOrigin(b3Vector3(0,-1.1,0)); + frameInB.setOrigin(b3Vector3(0,1.1,0)); + + c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB); + //c->setBreakingImpulseThreshold(37.1); + + break; + } + case 2: + { + b3Transform frameInA,frameInB; + frameInA.setIdentity(); + frameInB.setIdentity(); + frameInA.setOrigin(b3Vector3(0,-1.1,0)); + frameInB.setOrigin(b3Vector3(0,1.1,0)); + + b3Generic6DofConstraint* dof6 = new b3Generic6DofConstraint(pid,prevBody,frameInA,frameInB,false,m_data->m_np->getBodiesCpu()); + for (int i=0;i<6;i++) + dof6->setLimit(i,0,0); + c=dof6; + break; + } + default: + { + + b3Assert(0); + } + }; + if (c) + { + m_data->m_rigidBodyPipeline->addConstraint(c);//,false); + } + } + + prevBody = pid; index++; @@ -192,7 +241,7 @@ void GpuConstraintsDemo::createStaticEnvironment(const ConstructionInfo& ci) { b3Vector4 scaling(400,400,400,1); int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); - b3Vector3 position(0,-400,0); + b3Vector3 position(0,-405,0); b3Quaternion orn(0,0,0,1); b3Vector4 color(0,0,1,1); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp new file mode 100644 index 000000000..bc7aa3976 --- /dev/null +++ b/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp @@ -0,0 +1,108 @@ + +#include "b3FixedConstraint.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" +#include "Bullet3Common/b3TransformUtil.h" +#include + + +b3FixedConstraint::b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB) +:b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE,rbA,rbB) +{ + m_pivotInA = frameInA.getOrigin(); + m_pivotInB = frameInB.getOrigin(); + m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); + +} + +b3FixedConstraint::~b3FixedConstraint () +{ +} + + +void b3FixedConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies) +{ + info->m_numConstraintRows = 6; + info->nub = 6; +} + +void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies) +{ + //fix the 3 linear degrees of freedom + + const b3Vector3& worldPosA = bodies[m_rbA].m_pos; + const b3Quaternion& worldOrnA = bodies[m_rbA].m_quat; + const b3Vector3& worldPosB= bodies[m_rbB].m_pos; + const b3Quaternion& worldOrnB = bodies[m_rbB].m_quat; + + info->m_J1linearAxis[0] = 1; + info->m_J1linearAxis[info->rowskip+1] = 1; + info->m_J1linearAxis[2*info->rowskip+2] = 1; + + b3Vector3 a1 = b3QuatRotate(worldOrnA,m_pivotInA); + { + b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); + b3Vector3 a1neg = -a1; + a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + if (info->m_J2linearAxis) + { + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; + } + + b3Vector3 a2 = b3QuatRotate(worldOrnB,m_pivotInB); + + { + // b3Vector3 a2n = -a2; + b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); + b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); + b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); + a2.getSkewSymmetricMatrix(angular0,angular1,angular2); + } + + // set right hand side for the linear dofs + b3Scalar k = info->fps * info->erp; + b3Vector3 linearError = k*(a2+worldPosB-a1-worldPosA); + int j; + for (j=0; j<3; j++) + { + info->m_constraintError[j*info->rowskip] = linearError[j]; + //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); + } + + //fix the 3 angular degrees of freedom + + int start_row = 3; + int s = info->rowskip; + int start_index = start_row * s; + + // 3 rows to make body rotations equal + info->m_J1angularAxis[start_index] = 1; + info->m_J1angularAxis[start_index + s + 1] = 1; + info->m_J1angularAxis[start_index + s*2+2] = 1; + if ( info->m_J2angularAxis) + { + info->m_J2angularAxis[start_index] = -1; + info->m_J2angularAxis[start_index + s+1] = -1; + info->m_J2angularAxis[start_index + s*2+2] = -1; + } + + + // set right hand side for the angular dofs + + b3Vector3 diff; + b3Scalar angle; + b3Quaternion qrelCur = worldOrnA *worldOrnB.inverse(); + + b3TransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle); + diff*=-angle; + for (j=0; j<3; j++) + { + info->m_constraintError[(3+j)*info->rowskip] = k * diff[j]; + } + +} \ No newline at end of file diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h new file mode 100644 index 000000000..c811f095e --- /dev/null +++ b/src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h @@ -0,0 +1,35 @@ + +#ifndef B3_FIXED_CONSTRAINT_H +#define B3_FIXED_CONSTRAINT_H + +#include "b3TypedConstraint.h" + +B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint +{ + b3Vector3 m_pivotInA; + b3Vector3 m_pivotInB; + b3Quaternion m_relTargetAB; + +public: + b3FixedConstraint(int rbA,int rbB, const b3Transform& frameInA,const b3Transform& frameInB); + + virtual ~b3FixedConstraint(); + + + virtual void getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies); + + virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies); + + virtual void setParam(int num, b3Scalar value, int axis = -1) + { + b3Assert(0); + } + virtual b3Scalar getParam(int num, int axis = -1) const + { + b3Assert(0); + return 0.f; + } + +}; + +#endif //B3_FIXED_CONSTRAINT_H diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp index fe8a14723..7295e9b81 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp @@ -42,8 +42,7 @@ b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA,int rbB, const b3Trans , m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), -m_flags(0), -m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD) +m_flags(0) { calculateTransforms(bodies); } @@ -275,56 +274,42 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index) void b3Generic6DofConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies) { - if (m_useSolveConstraintObsolete) + //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++) { - 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)) { - 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--; - } + info->m_numConstraintRows++; + info->nub--; } } + //test angular limits + for (i=0;i<3 ;i++ ) + { + if(testAngularLimitMotor(i)) + { + info->m_numConstraintRows++; + info->nub--; + } + } +// printf("info->m_numConstraintRows=%d\n",info->m_numConstraintRows); } 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; - } + //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]); @@ -350,7 +335,6 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod 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); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h index 1c95d52c0..02bb1a82d 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h @@ -299,12 +299,12 @@ protected: protected: //! temporal variables //!@{ - b3Scalar m_timeStep; b3Transform m_calculatedTransformA; b3Transform m_calculatedTransformB; b3Vector3 m_calculatedAxisAngleDiff; b3Vector3 m_calculatedAxis[3]; b3Vector3 m_calculatedLinearDiff; + b3Scalar m_timeStep; b3Scalar m_factA; b3Scalar m_factB; bool m_hasStaticBody; @@ -343,9 +343,6 @@ 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 diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp index 07735ad94..6a8112815 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -25,8 +25,7 @@ subject to the following restrictions: b3Point2PointConstraint::b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB) :b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), -m_flags(0), -m_useSolveConstraintObsolete(false) +m_flags(0) { } @@ -49,15 +48,8 @@ void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info,const b3RigidBod void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const b3RigidBodyCL* bodies) { - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } else - { info->m_numConstraintRows = 3; info->nub = 3; - } } @@ -80,7 +72,6 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans) { - b3Assert(!m_useSolveConstraintObsolete); //retrieve matrices @@ -92,6 +83,8 @@ void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const info->m_J1linearAxis[2*info->rowskip+2] = 1; b3Vector3 a1 = body0_trans.getBasis()*getPivotInA(); + b3Vector3 a1a = b3QuatRotate(body0_trans.getRotation(),getPivotInA()); + { b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h index 6e8447102..5499a409e 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h @@ -68,9 +68,6 @@ public: B3_DECLARE_ALIGNED_ALLOCATOR(); - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - b3ConstraintSetting m_setting; b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h b/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h index 7e8503e1b..0bded5b0c 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h @@ -33,6 +33,7 @@ enum b3TypedConstraintType B3_CONTACT_CONSTRAINT_TYPE, B3_D6_SPRING_CONSTRAINT_TYPE, B3_GEAR_CONSTRAINT_TYPE, + B3_FIXED_CONSTRAINT_TYPE, B3_MAX_CONSTRAINT_TYPE };