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

add b3FixedConstraint, with less memory footprint than b3Generic6DofConstraint, and easier to port to OpenCL.

Browse Source 
remove the 'obsolete' part of b3Generic6DofConstraint and b3Point2PointConstraint
This commit is contained in:
erwin coumans
2013-07-06 00:44:06 -07:00
parent 956c3fc267
commit 01e5472f9f
8 changed files with 226 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp Normal file
View File

@@ -0,0 +1,108 @@
#include "b3FixedConstraint.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
#include "Bullet3Common/b3TransformUtil.h"
#include <new>
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];
}
}

35
src/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h Normal file
View File

@@ -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

64
src/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp
View File

@@ -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);

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

@@ -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

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

@@ -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);

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

@@ -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);

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

@@ -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
};