// Clips a face to the back of a plane
void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS)
{
int ve;
btScalar ds, de;
int numVerts = pVtxIn.size();
if (numVerts < 2)
return;
btVector3 firstVertex=pVtxIn[pVtxIn.size()-1];
btVector3 endVertex = pVtxIn[0];
ds = planeNormalWS.dot(firstVertex)+planeEqWS;
for (ve = 0; ve < numVerts; ve++)
{
endVertex=pVtxIn[ve];
de = planeNormalWS.dot(endVertex)+planeEqWS;
if (ds<0)
{
if (de<0)
{
// Start < 0, end < 0, so output endVertex
ppVtxOut.push_back(endVertex);
}
else
{
// Start < 0, end >= 0, so output intersection
ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
}
}
else
{
if (de<0)
{
// Start >= 0, end < 0 so output intersection and end
ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de))));
ppVtxOut.push_back(endVertex);
}
}
firstVertex = endVertex;
ds = de;
}
}
inline btVector3 findNearestPointToLine(const btVector3 &pt1, const btVector3 &pt2, const btVector3 &testPoint, float &uDistance)
{
const btVector3 A = testPoint - pt1;
const btVector3 u = (pt2-pt1).normalized();
uDistance = A.dot(u);
return pt1 + uDistance * u;
};
void get_plane_equation_transformed(const btTransform& trans, btVector4& equation) const
{
const btVector3 normal = trans.getBasis() * m_planeNormal;
equation[0] = normal[0];
equation[1] = normal[1];
equation[2] = normal[2];
equation[3] = normal.dot(trans * (m_planeConstant * m_planeNormal));
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
if (projection >= 1.0f - SIMD_EPSILON) {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else if (projection <= -1.0f + SIMD_EPSILON) {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
//bilateral constraint between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
{
(void)timeStep;
(void)distance;
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1));
if (normalLenSqr > btScalar(1.1))
{
impulse = btScalar(0.);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity(
body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
btScalar a;
a=jacDiagABInv;
rel_vel = normal.dot(vel);
//todo: move this into proper structure
btScalar contactDamping = btScalar(0.2);
#ifdef ONLY_USE_LINEAR_MASS
btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
impulse = - contactDamping * rel_vel * massTerm;
#else
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
#if 1
minProj = FLT_MAX;
maxProj = -FLT_MAX;
int numVerts = m_unscaledPoints.size();
for(int i=0;i<numVerts;i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
btVector3 pt = trans * vtx;
btScalar dp = pt.dot(dir);
if(dp < minProj)
{
minProj = dp;
witnesPtMin = pt;
}
if(dp > maxProj)
{
maxProj = dp;
witnesPtMax=pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
witnesPtMin = trans(localGetSupportingVertex(localAxis));
witnesPtMax = trans(localGetSupportingVertex(-localAxis));
minProj = witnesPtMin.dot(dir);
maxProj = witnesPtMax.dot(dir);
#endif
if(minProj>maxProj)
{
btSwap(minProj,maxProj);
btSwap(witnesPtMin,witnesPtMax);
}
}
bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
{
int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++)
{
const btVector3& N1 = planeEquations[i];
if (planeEquation.dot(N1) > btScalar(0.999))
{
return false;
}
}
return true;
}
void btSliderConstraint::testAngLimits(void)
{
m_angDepth = btScalar(0.);
m_solveAngLim = false;
if(m_lowerAngLimit <= m_upperAngLimit)
{
const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
if(rot < m_lowerAngLimit)
{
m_angDepth = rot - m_lowerAngLimit;
m_solveAngLim = true;
}
else if(rot > m_upperAngLimit)
{
m_angDepth = rot - m_upperAngLimit;
m_solveAngLim = true;
}
}
} // btSliderConstraint::testAngLimits()
bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin)
{
int numvertices = vertices.size();
for (int i=0;i<numvertices;i++)
{
const btVector3& N1 = vertices[i];
btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
if (dist>btScalar(0.))
{
return false;
}
}
return true;
}
virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex)
{
(void)partId;
(void)triangleIndex;
for (int i=0;i<3;i++)
{
btScalar dot = m_supportVecLocal.dot(triangle[i]);
if (dot > m_maxDot)
{
m_maxDot = dot;
m_supportVertexLocal = triangle[i];
}
}
}
bool EpaFace::Initialize()
{
assert( m_pHalfEdge && "Must setup half-edge first!" );
CollectVertices( m_pVertices );
const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
const btScalar e0Sqrd = e0.length2();
const btScalar e1Sqrd = e1.length2();
const btScalar e0e1 = e0.dot( e1 );
m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
const btScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
const btScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
if ( IsAffinelyDependent() )
{
return false;
}
CalcClosestPoint();
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( !CalculatePlane() )
{
return false;
}
#endif
return true;
}
void collisionWithBound(int i, btVector3& normal)
{
static const float sElasticity = 0.01f ;
static const float sImpactCoefficient = 1.0f + sElasticity ;
btFluidParticles& particles = fluid->internalGetParticles();
btVector3 velFluid = fluid->getVelocity(i);
const btVector3 velRelative = velFluid;
const btScalar speedNormal = velRelative.dot(normal); // Contact normal depends on geometry.
const btVector3 impulse = - speedNormal * normal ; // Minus: speedNormal is negative.
btVector3& vel = particles.m_vel[i];
//Leapfrog integration
btVector3 velNext = vel + impulse * sImpactCoefficient;
vel = velNext;
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
if (projection >= 1.0f - SIMD_EPSILON) {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else if (projection <= -1.0f + SIMD_EPSILON) {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
}
btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
for (int i=0;i<m_unscaledPoints.size();i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
btVector3 separatingNormal = separatingNormal1.normalized();
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
btScalar curMaxDist=maxDist;
int closestFaceB=-1;
btScalar dmax = -FLT_MAX;
{
for(int face=0;face<hullB.m_faces.size();face++)
{
const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
btScalar d = WorldNormal.dot(separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
btVertexArray worldVertsB1;
{
const btFace& polyB = hullB.m_faces[closestFaceB];
const int numVertices = polyB.m_indices.size();
for(int e0=0;e0<numVertices;e0++)
{
const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
worldVertsB1.push_back(transB*b);
}
}
if (closestFaceB>=0)
clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
}
void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
// transforms in world space
btTransform trA = transA*m_rbAFrame;
btTransform trB = transB*m_rbBFrame;
// pivot point
btVector3 pivotAInW = trA.getOrigin();
btVector3 pivotBInW = trB.getOrigin();
#if 1
// difference between frames in WCS
btVector3 ofs = trB.getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
btScalar miA = getRigidBodyA().getInvMass();
btScalar miB = getRigidBodyB().getInvMass();
bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
btScalar factA, factB;
if(miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
factB = btScalar(1.0f) - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
btVector3 ax1A = trA.getBasis().getColumn(2);
btVector3 ax1B = trB.getBasis().getColumn(2);
btVector3 ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
int s0 = 0;
int s1 = s;
int s2 = s * 2;
int nrow = 2; // last filled row
btVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to hinge axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to hinge axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = trA.getOrigin() - bodyA_trans.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
btVector3 totalDist = projA - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to hinge axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
if(len2 > SIMD_EPSILON)
{
p /= btSqrt(len2);
}
else
{
p = trA.getBasis().getColumn(1);
}
// make one more ortho
q = ax1.cross(p);
// fill three rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
tmpA = relA.cross(q);
tmpB = relB.cross(q);
if(hasStaticBody && getSolveLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
if(hasStaticBody)
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
// compute three elements of right hand side
btScalar k = info->fps * info->erp;
btScalar rhs = k * p.dot(ofs);
info->m_constraintError[s0] = rhs;
rhs = k * q.dot(ofs);
info->m_constraintError[s1] = rhs;
rhs = k * ax1.dot(ofs);
info->m_constraintError[s2] = rhs;
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
int s3 = 3 * s;
int s4 = 4 * s;
info->m_J1angularAxis[s3 + 0] = p[0];
info->m_J1angularAxis[s3 + 1] = p[1];
info->m_J1angularAxis[s3 + 2] = p[2];
info->m_J1angularAxis[s4 + 0] = q[0];
info->m_J1angularAxis[s4 + 1] = q[1];
info->m_J1angularAxis[s4 + 2] = q[2];
info->m_J2angularAxis[s3 + 0] = -p[0];
info->m_J2angularAxis[s3 + 1] = -p[1];
info->m_J2angularAxis[s3 + 2] = -p[2];
info->m_J2angularAxis[s4 + 0] = -q[0];
info->m_J2angularAxis[s4 + 1] = -q[1];
info->m_J2angularAxis[s4 + 2] = -q[2];
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info->fps * info->erp;
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
#endif
// check angular limits
nrow = 4; // last filled row
int srow;
btScalar limit_err = btScalar(0.0);
int limit = 0;
if(getSolveLimit())
{
limit_err = m_correction * m_referenceSign;
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
int powered = 0;
if(getEnableAngularMotor())
{
powered = 1;
}
if(limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
if(powered)
{
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
info->m_lowerLimit[srow] = - m_maxMotorImpulse;
info->m_upperLimit[srow] = m_maxMotorImpulse;
}
if(limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
if(lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{ // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
btScalar bounce = m_relaxationFactor;
if(bounce > btScalar(0.0))
{
btScalar vel = angVelA.dot(ax1);
vel -= angVelB.dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if(newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= m_biasFactor;
} // if(limit)
} // if angular limit or powered
}
void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
{
btAssert(!m_useSolveConstraintObsolete);
int i, skip = info->rowskip;
// transforms in world space
btTransform trA = transA*m_rbAFrame;
btTransform trB = transB*m_rbBFrame;
// pivot point
btVector3 pivotAInW = trA.getOrigin();
btVector3 pivotBInW = trB.getOrigin();
#if 0
if (0)
{
for (i=0;i<6;i++)
{
info->m_J1linearAxis[i*skip]=0;
info->m_J1linearAxis[i*skip+1]=0;
info->m_J1linearAxis[i*skip+2]=0;
info->m_J1angularAxis[i*skip]=0;
info->m_J1angularAxis[i*skip+1]=0;
info->m_J1angularAxis[i*skip+2]=0;
info->m_J2angularAxis[i*skip]=0;
info->m_J2angularAxis[i*skip+1]=0;
info->m_J2angularAxis[i*skip+2]=0;
info->m_constraintError[i*skip]=0.f;
}
}
#endif //#if 0
// linear (all fixed)
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[skip + 1] = 1;
info->m_J1linearAxis[2 * skip + 2] = 1;
btVector3 a1 = pivotAInW - transA.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
btVector3 a2 = pivotBInW - transB.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// linear RHS
btScalar k = info->fps * info->erp;
for(i = 0; i < 3; i++)
{
info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
}
// make rotations around X and Y equal
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
// get hinge axis (Z)
btVector3 ax1 = trA.getBasis().getColumn(2);
// get 2 orthos to hinge axis (X, Y)
btVector3 p = trA.getBasis().getColumn(0);
btVector3 q = trA.getBasis().getColumn(1);
// set the two hinge angular rows
int s3 = 3 * info->rowskip;
int s4 = 4 * info->rowskip;
info->m_J1angularAxis[s3 + 0] = p[0];
info->m_J1angularAxis[s3 + 1] = p[1];
info->m_J1angularAxis[s3 + 2] = p[2];
info->m_J1angularAxis[s4 + 0] = q[0];
info->m_J1angularAxis[s4 + 1] = q[1];
info->m_J1angularAxis[s4 + 2] = q[2];
info->m_J2angularAxis[s3 + 0] = -p[0];
info->m_J2angularAxis[s3 + 1] = -p[1];
info->m_J2angularAxis[s3 + 2] = -p[2];
info->m_J2angularAxis[s4 + 0] = -q[0];
info->m_J2angularAxis[s4 + 1] = -q[1];
info->m_J2angularAxis[s4 + 2] = -q[2];
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
btVector3 ax2 = trB.getBasis().getColumn(2);
btVector3 u = ax1.cross(ax2);
info->m_constraintError[s3] = k * u.dot(p);
info->m_constraintError[s4] = k * u.dot(q);
// check angular limits
int nrow = 4; // last filled row
int srow;
btScalar limit_err = btScalar(0.0);
int limit = 0;
if(getSolveLimit())
{
limit_err = m_correction * m_referenceSign;
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
int powered = 0;
if(getEnableAngularMotor())
{
powered = 1;
}
if(limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
btScalar lostop = getLowerLimit();
btScalar histop = getUpperLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
if(powered)
{
if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
{
info->cfm[srow] = m_normalCFM;
}
btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
info->m_lowerLimit[srow] = - m_maxMotorImpulse;
info->m_upperLimit[srow] = m_maxMotorImpulse;
}
if(limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
{
info->cfm[srow] = m_stopCFM;
}
if(lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{ // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
btScalar bounce = m_relaxationFactor;
if(bounce > btScalar(0.0))
{
btScalar vel = angVelA.dot(ax1);
vel -= angVelB.dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if(newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= m_biasFactor;
} // if(limit)
} // if angular limit or powered
}
btScalar btTranslationalLimitMotor::solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos)
{
///find relative velocity
// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
btVector3 vel1;
body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2;
body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel = axis_normal_on_a.dot(vel);
/// apply displacement correction
//positional error (zeroth order error)
btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
btScalar lo = btScalar(-BT_LARGE_FLOAT);
btScalar hi = btScalar(BT_LARGE_FLOAT);
btScalar minLimit = m_lowerLimit[limit_index];
btScalar maxLimit = m_upperLimit[limit_index];
//handle the limits
if (minLimit < maxLimit)
{
{
if (depth > maxLimit)
{
depth -= maxLimit;
lo = btScalar(0.);
}
else
{
if (depth < minLimit)
{
depth -= minLimit;
hi = btScalar(0.);
}
else
{
return 0.0f;
}
}
}
}
btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
btScalar sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
return normalImpulse;
}
btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
btRigidBody * body0, btRigidBody * body1 )
{
if (needApplyTorques()==false) return 0.0f;
btScalar target_velocity = m_targetVelocity;
btScalar maxMotorForce = m_maxMotorForce;
//current error correction
if (m_currentLimit!=0)
{
target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
btVector3 angVelA;
body0->internalGetAngularVelocity(angVelA);
btVector3 angVelB;
body1->internalGetAngularVelocity(angVelB);
btVector3 vel_diff;
vel_diff = angVelA-angVelB;
btScalar rel_vel = axis.dot(vel_diff);
// correction velocity
btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
// clip correction impulse
btScalar clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if (unclippedMotorImpulse>0.0f)
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
}
// sort with accumulated impulses
btScalar lo = btScalar(-BT_LARGE_FLOAT);
btScalar hi = btScalar(BT_LARGE_FLOAT);
btScalar oldaccumImpulse = m_accumulatedImpulse;
btScalar sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
btVector3 motorImp = clippedMotorImpulse * axis;
//body0->applyTorqueImpulse(motorImp);
//body1->applyTorqueImpulse(-motorImp);
body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
return clippedMotorImpulse;
}
void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass )
{
const btTransform& trA = getCalculatedTransformA();
const btTransform& trB = getCalculatedTransformB();
btAssert(!m_useSolveConstraintObsolete);
int i, s = info->rowskip;
btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
// difference between frames in WCS
btVector3 ofs = trB.getOrigin() - trA.getOrigin();
// now get weight factors depending on masses
btScalar miA = rbAinvMass;
btScalar miB = rbBinvMass;
bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
btScalar miS = miA + miB;
btScalar factA, factB;
if(miS > btScalar(0.f))
{
factA = miB / miS;
}
else
{
factA = btScalar(0.5f);
}
factB = btScalar(1.0f) - factA;
btVector3 ax1, p, q;
btVector3 ax1A = trA.getBasis().getColumn(0);
btVector3 ax1B = trB.getBasis().getColumn(0);
if(m_useOffsetForConstraintFrame)
{
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// construct two orthos to slider axis
btPlaneSpace1 (ax1, p, q);
}
else
{ // old way - use frameA
ax1 = trA.getBasis().getColumn(0);
// get 2 orthos to slider axis (Y, Z)
p = trA.getBasis().getColumn(1);
q = trA.getBasis().getColumn(2);
}
// make rotations around these orthos equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the slider axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the slider axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
info->m_J1angularAxis[0] = p[0];
info->m_J1angularAxis[1] = p[1];
info->m_J1angularAxis[2] = p[2];
info->m_J1angularAxis[s+0] = q[0];
info->m_J1angularAxis[s+1] = q[1];
info->m_J1angularAxis[s+2] = q[2];
info->m_J2angularAxis[0] = -p[0];
info->m_J2angularAxis[1] = -p[1];
info->m_J2angularAxis[2] = -p[2];
info->m_J2angularAxis[s+0] = -q[0];
info->m_J2angularAxis[s+1] = -q[1];
info->m_J2angularAxis[s+2] = -q[2];
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment.
// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
// btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
btScalar k = info->fps * currERP;
btVector3 u = ax1A.cross(ax1B);
info->m_constraintError[0] = k * u.dot(p);
info->m_constraintError[s] = k * u.dot(q);
if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
{
info->cfm[0] = m_cfmOrthoAng;
info->cfm[s] = m_cfmOrthoAng;
}
int nrow = 1; // last filled row
int srow;
btScalar limit_err;
int limit;
int powered;
// next two rows.
// we want: velA + wA x relA == velB + wB x relB ... but this would
// result in three equations, so we project along two orthos to the slider axis
btTransform bodyA_trans = transA;
btTransform bodyB_trans = transB;
nrow++;
int s2 = nrow * s;
nrow++;
int s3 = nrow * s;
btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
if(m_useOffsetForConstraintFrame)
{
// get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to slider axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to slider axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = trA.getOrigin() - bodyA_trans.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
btScalar sliderOffs = m_linPos - m_depth[0];
// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
btVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
if(len2 > SIMD_EPSILON)
{
p /= btSqrt(len2);
}
else
{
p = trA.getBasis().getColumn(1);
}
// make one more ortho
q = ax1.cross(p);
// fill two rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
tmpA = relA.cross(q);
tmpB = relB.cross(q);
if(hasStaticBody && getSolveAngLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
}
else
{ // old way - maybe incorrect if bodies are not on the slider axis
// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
btVector3 tmp = c.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
tmp = c.cross(q);
for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
}
// compute two elements of right hand side
// k = info->fps * info->erp * getSoftnessOrthoLin();
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
k = info->fps * currERP;
btScalar rhs = k * p.dot(ofs);
info->m_constraintError[s2] = rhs;
rhs = k * q.dot(ofs);
info->m_constraintError[s3] = rhs;
if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
{
info->cfm[s2] = m_cfmOrthoLin;
info->cfm[s3] = m_cfmOrthoLin;
}
// check linear limits
limit_err = btScalar(0.0);
limit = 0;
if(getSolveLinLimit())
{
limit_err = getLinDepth() * signFact;
limit = (limit_err > btScalar(0.0)) ? 2 : 1;
}
powered = 0;
if(getPoweredLinMotor())
{
powered = 1;
}
// if the slider has joint limits or motor, add in the extra row
if (limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1linearAxis[srow+0] = ax1[0];
info->m_J1linearAxis[srow+1] = ax1[1];
info->m_J1linearAxis[srow+2] = ax1[2];
info->m_J2linearAxis[srow+0] = -ax1[0];
info->m_J2linearAxis[srow+1] = -ax1[1];
info->m_J2linearAxis[srow+2] = -ax1[2];
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
if(m_useOffsetForConstraintFrame)
{
// this is needed only when bodyA and bodyB are both dynamic.
if(!hasStaticBody)
{
tmpA = relA.cross(ax1);
tmpB = relB.cross(ax1);
info->m_J1angularAxis[srow+0] = tmpA[0];
info->m_J1angularAxis[srow+1] = tmpA[1];
info->m_J1angularAxis[srow+2] = tmpA[2];
info->m_J2angularAxis[srow+0] = -tmpB[0];
info->m_J2angularAxis[srow+1] = -tmpB[1];
info->m_J2angularAxis[srow+2] = -tmpB[2];
}
}
else
{ // The old way. May be incorrect if bodies are not on the slider axis
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = factA*ltd[0];
info->m_J1angularAxis[srow+1] = factA*ltd[1];
info->m_J1angularAxis[srow+2] = factA*ltd[2];
info->m_J2angularAxis[srow+0] = factB*ltd[0];
info->m_J2angularAxis[srow+1] = factB*ltd[1];
info->m_J2angularAxis[srow+2] = factB*ltd[2];
}
// right-hand part
btScalar lostop = getLowerLinLimit();
btScalar histop = getUpperLinLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
info->m_constraintError[srow] = 0.;
info->m_lowerLimit[srow] = 0.;
info->m_upperLimit[srow] = 0.;
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
if(powered)
{
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
{
info->cfm[srow] = m_cfmDirLin;
}
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
}
if(limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
{
info->cfm[srow] = m_cfmLimLin;
}
if(lostop == histop)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
else
{ // high limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
if(bounce > btScalar(0.0))
{
btScalar vel = linVelA.dot(ax1);
vel -= linVelB.dot(ax1);
vel *= signFact;
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if (newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= getSoftnessLimLin();
} // if(limit)
} // if linear limit
// check angular limits
limit_err = btScalar(0.0);
limit = 0;
if(getSolveAngLimit())
{
limit_err = getAngDepth();
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the slider has joint limits, add in the extra row
powered = 0;
if(getPoweredAngMotor())
{
powered = 1;
}
if(limit || powered)
{
nrow++;
srow = nrow * info->rowskip;
info->m_J1angularAxis[srow+0] = ax1[0];
info->m_J1angularAxis[srow+1] = ax1[1];
info->m_J1angularAxis[srow+2] = ax1[2];
info->m_J2angularAxis[srow+0] = -ax1[0];
info->m_J2angularAxis[srow+1] = -ax1[1];
info->m_J2angularAxis[srow+2] = -ax1[2];
btScalar lostop = getLowerAngLimit();
btScalar histop = getUpperAngLimit();
if(limit && (lostop == histop))
{ // the joint motor is ineffective
powered = 0;
}
currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
if(powered)
{
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
{
info->cfm[srow] = m_cfmDirAng;
}
btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
}
if(limit)
{
k = info->fps * currERP;
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
{
info->cfm[srow] = m_cfmLimAng;
}
if(lostop == histop)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else if(limit == 1)
{ // low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{ // high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
if(bounce > btScalar(0.0))
{
btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
vel -= m_rbB.getAngularVelocity().dot(ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if(limit == 1)
{ // low limit
if(vel < 0)
{
btScalar newc = -bounce * vel;
if(newc > info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if(vel > 0)
{
btScalar newc = -bounce * vel;
if(newc < info->m_constraintError[srow])
{
info->m_constraintError[srow] = newc;
}
}
}
}
info->m_constraintError[srow] *= getSoftnessLimAng();
} // if(limit)
} // if angular limit or powered
}
void CustomConvexConvexPairCollision::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
#if 0
if (!m_manifoldPtr)
{
//swapped?
m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
m_ownManifold = true;
}
resultOut->setPersistentManifold(m_manifoldPtr);
CustomConvexShape* convex0 = (CustomConvexShape*)body0->getCollisionShape();
CustomConvexShape* convex1 = (CustomConvexShape*)body1->getCollisionShape();
float4 bodyApos;
float4 bodyBpos;
Quaternion bodyAquat;
Quaternion bodyBquat;
const btTransform& transA = body0->getWorldTransform();
const btTransform& transB = body1->getWorldTransform();
const btVector3& pA = body0->getWorldTransform().getOrigin();
const btVector3& pB = body1->getWorldTransform().getOrigin();
btQuaternion qA = body0->getWorldTransform().getRotation();
btQuaternion qB = body1->getWorldTransform().getRotation();
bodyApos.x = pA.getX();
bodyApos.y = pA.getY();
bodyApos.z = pA.getZ();
bodyApos.w = 0.f;
bodyBpos.x = pB.getX();
bodyBpos.y = pB.getY();
bodyBpos.z = pB.getZ();
bodyBpos.w = 0.f;
bodyAquat.x = qA.getX();
bodyAquat.y = qA.getY();
bodyAquat.z = qA.getZ();
bodyAquat.w = qA.getW();
bodyBquat.x = qB.getX();
bodyBquat.y = qB.getY();
bodyBquat.z = qB.getZ();
bodyBquat.w = qB.getW();
#define CAPACITY_CONTACTS 4
ContactPoint4 contactsOut[CAPACITY_CONTACTS];
int freeContactIndex = 0;
int contactCapacity = CAPACITY_CONTACTS;
float collisionMargin = 0.001f;
m_manifoldPtr->refreshContactPoints(body0->getWorldTransform(),body1->getWorldTransform());
collideStraight(convex0->m_ConvexHeightField,convex1->m_ConvexHeightField,
bodyApos, bodyAquat,bodyBpos,bodyBquat,
contactsOut, freeContactIndex, contactCapacity,
collisionMargin );
collideStraight(convex1->m_ConvexHeightField,convex0->m_ConvexHeightField,
bodyBpos, bodyBquat,bodyApos,bodyAquat,
contactsOut, freeContactIndex, contactCapacity,
collisionMargin );
//copy points into manifold
//refresh manifold
btAssert(freeContactIndex<3);
for (int j=0; j<freeContactIndex; j++)
{
int numPoints = contactsOut[j].getNPoints();
// printf("numPoints = %d\n",numPoints);
for (int i=0; i<numPoints; i++)
{
ContactPoint4& c = contactsOut[j];
btVector3 normalOnBInWorld(
c.m_worldNormal.x,
c.m_worldNormal.y,
c.m_worldNormal.z);
btVector3 pointInWorldOnB(
c.m_worldPos[i].x,
c.m_worldPos[i].y,
c.m_worldPos[i].z);
btScalar depth = c.m_worldPos[i].w;
if (depth<0)
{
const btVector3 deltaC = transB.getOrigin() - transA.getOrigin();
if((deltaC.dot(normalOnBInWorld))>0.0f)
{
normalOnBInWorld= -normalOnBInWorld;
}
normalOnBInWorld.normalize();
if (j)
{
resultOut->addContactPoint(normalOnBInWorld, pointInWorldOnB, depth);
} else
{
resultOut->addContactPoint(normalOnBInWorld, pointInWorldOnB-normalOnBInWorld*depth, depth);
}
}
}
}
#else
btConvexConvexAlgorithm::processCollision(body0,body1,dispatchInfo,resultOut);
#endif
}
void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
btVertexArray worldVertsB2;
btVertexArray* pVtxIn = &worldVertsB1;
btVertexArray* pVtxOut = &worldVertsB2;
pVtxOut->reserve(pVtxIn->size());
int closestFaceA=-1;
{
btScalar dmin = FLT_MAX;
for(int face=0;face<hullA.m_faces.size();face++)
{
const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
const btVector3 faceANormalWS = transA.getBasis() * Normal;
btScalar d = faceANormalWS.dot(separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA<0)
return;
const btFace& polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn->size();
int numVerticesA = polyA.m_indices.size();
for(int e0=0;e0<numVerticesA;e0++)
{
const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]];
const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]];
const btVector3 edge0 = a - b;
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0);
btVector3 worldA1 = transA*a;
btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1);
//int otherFace=0;
#ifdef BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
#else
btVector3 planeNormalWS = planeNormalWS1;
btScalar planeEqWS=planeEqWS1;
#endif
//clip face
clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
btSwap(pVtxIn,pVtxOut);
pVtxOut->resize(0);
}
//#define ONLY_REPORT_DEEPEST_POINT
btVector3 point;
// only keep points that are behind the witness face
{
btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]);
btScalar localPlaneEq = polyA.m_plane[3];
btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal;
btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin());
for (int i=0;i<pVtxIn->size();i++)
{
btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS;
if (depth <=minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <=maxDist)
{
btVector3 point = pVtxIn->at(i);
#ifdef ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if 0
if (depth<-3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.addContactPoint(separatingNormal,point,depth);
#endif
}
}
}
#ifdef ONLY_REPORT_DEEPEST_POINT
if (curMaxDist<maxDist)
{
resultOut.addContactPoint(separatingNormal,point,curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
// Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal'
// from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
btVector3 CCharacterController::ComputeReflectionDirection(const btVector3& direction, const btVector3& normal)
{
return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
}
void collisionWithBound(btVector3 &vel, btVector3 &pos, const btVector3 &normal, const btScalar &penetrationDist)
{
if( penetrationDist < btScalar(0.0) )
{
pos = pos - normal * penetrationDist;
vel = vel - (1 + Constant.m_boundaryRestitution * (-penetrationDist) / (Constant.m_timeStep * vel.length())) * vel.dot(normal) * normal;
}
}
void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
btMultiBodyJacobianData& data,
const btVector3& contactNormalOnB,
const btVector3& posAworld, const btVector3& posBworld,
btScalar position,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
btVector3 rel_pos1 = posAworld;
btVector3 rel_pos2 = posBworld;
solverConstraint.m_multiBodyA = m_bodyA;
solverConstraint.m_multiBodyB = m_bodyB;
solverConstraint.m_linkA = m_linkA;
solverConstraint.m_linkB = m_linkB;
btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
const btVector3& pos1 = posAworld;
const btVector3& pos2 = posBworld;
btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
if (bodyA)
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
if (bodyB)
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f;
if (multiBodyA)
{
const int ndofA = multiBodyA->getNumLinks() + 6;
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
if (solverConstraint.m_deltaVelAindex <0)
{
solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
} else
{
btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
}
solverConstraint.m_jacAindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
} else
{
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
solverConstraint.m_contactNormal1 = contactNormalOnB;
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
if (solverConstraint.m_deltaVelBindex <0)
{
solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
}
solverConstraint.m_jacBindex = data.m_jacobians.size();
data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
} else
{
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
solverConstraint.m_contactNormal2 = -contactNormalOnB;
}
{
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
btScalar* jacB = 0;
btScalar* jacA = 0;
btScalar* lambdaA =0;
btScalar* lambdaB =0;
int ndofA = 0;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA; ++i)
{
btScalar j = jacA[i] ;
btScalar l =lambdaA[i];
denom0 += j*l;
}
} else
{
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
}
}
if (multiBodyB)
{
const int ndofB = multiBodyB->getNumLinks() + 6;
jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB; ++i)
{
btScalar j = jacB[i] ;
btScalar l =lambdaB[i];
denom1 += j*l;
}
} else
{
if (rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
}
}
if (multiBodyA && (multiBodyA==multiBodyB))
{
// ndof1 == ndof2 in this case
for (int i = 0; i < ndofA; ++i)
{
denom1 += jacB[i] * lambdaA[i];
denom1 += jacA[i] * lambdaB[i];
}
}
btScalar d = denom0+denom1;
if (btFabs(d)>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
} else
{
solverConstraint.m_jacDiagABInv = 1.f;
}
}
//compute rhs and remaining solverConstraint fields
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
{
btVector3 vel1,vel2;
if (multiBodyA)
{
ndofA = multiBodyA->getNumLinks() + 6;
btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
for (int i = 0; i < ndofA ; ++i)
rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
} else
{
if (rb0)
{
rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
}
}
if (multiBodyB)
{
ndofB = multiBodyB->getNumLinks() + 6;
btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
for (int i = 0; i < ndofB ; ++i)
rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
} else
{
if (rb1)
{
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
}
solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
///warm starting (or zero if disabled)
/*
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (solverConstraint.m_appliedImpulse)
{
if (multiBodyA)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
multiBodyA->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
} else
{
if (rb0)
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
}
if (multiBodyB)
{
btScalar impulse = solverConstraint.m_appliedImpulse;
btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
multiBodyB->applyDeltaVee(deltaV,impulse);
applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
} else
{
if (rb1)
bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
}
}
} else
*/
{
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
solverConstraint.m_upperLimit = m_maxAppliedImpulse;
}
}
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
{
btVector3 vec = axis * invInertiaWorld;
return axis.dot(vec);
}
void Raytracer::displayCallback()
{
updateCamera();
for (int i=0;i<numObjects;i++)
{
transforms[i].setIdentity();
btVector3 pos(0.f,0.f,-(2.5* numObjects * 0.5)+i*2.5f);
transforms[i].setOrigin( pos );
btQuaternion orn;
if (i < 2)
{
orn.setEuler(yaw,pitch,roll);
transforms[i].setRotation(orn);
}
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
if (!m_initialized)
{
m_initialized = true;
glGenTextures(1, &glTextureId);
}
glBindTexture(GL_TEXTURE_2D,glTextureId );
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glDisable(GL_TEXTURE_2D);
glDisable(GL_BLEND);
btVector4 rgba(1.f,0.f,0.f,0.5f);
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = 2.0 * atanf (tanFov);
btVector3 rayFrom = getCameraPosition();
btVector3 rayForward = getCameraTargetPosition()-getCameraPosition();
rayForward.normalize();
float farPlane = 600.f;
rayForward*= farPlane;
btVector3 rightOffset;
btVector3 vertical(0.f,1.f,0.f);
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/float(screenWidth);
btVector3 dVert = vertical * 1.f/float(screenHeight);
btTransform rayFromTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFrom);
btTransform rayFromLocal;
btTransform rayToLocal;
int x;
///clear texture
for (x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
btVector4 rgba(0.2f,0.2f,0.2f,1.f);
raytracePicture->setPixel(x,y,rgba);
}
}
#if 1
btVector3 rayTo;
btTransform colObjWorldTransform;
colObjWorldTransform.setIdentity();
int mode = 0;
for (x=0;x<screenWidth;x++)
{
for (int y=0;y<screenHeight;y++)
{
rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += x * dHor;
rayTo -= y * dVert;
btVector3 worldNormal(0,0,0);
btVector3 worldPoint(0,0,0);
bool hasHit = false;
int mode = 0;
switch (mode)
{
case 0:
hasHit = lowlevelRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
case 1:
hasHit = singleObjectRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
case 2:
hasHit = worldRaytest(rayFrom,rayTo,worldNormal,worldPoint);
break;
default:
{
}
}
if (hasHit)
{
float lightVec0 = worldNormal.dot(btVector3(0,-1,-1));//0.4f,-1.f,-0.4f));
float lightVec1= worldNormal.dot(btVector3(-1,0,-1));//-0.4f,-1.f,-0.4f));
rgba = btVector4(lightVec0,lightVec1,0,1.f);
rgba.setMin(btVector3(1,1,1));
rgba.setMax(btVector3(0.2,0.2,0.2));
rgba[3] = 1.f;
raytracePicture->setPixel(x,y,rgba);
} else
btVector4 rgba = raytracePicture->getPixel(x,y);
if (!rgba.length2())
{
raytracePicture->setPixel(x,y,btVector4(1,1,1,1));
}
}
}
#endif
extern unsigned char sFontData[];
if (0)
{
const char* text="ABC abc 123 !@#";
int x=0;
for (int cc = 0;cc<strlen(text);cc++)
{
char testChar = text[cc];//'b';
char ch = testChar-32;
int startx=ch%16;
int starty=ch/16;
//for (int i=0;i<256;i++)
for (int i=startx*16;i<(startx*16+16);i++)
{
int y=0;
//for (int j=0;j<256;j++)
//for (int j=0;j<256;j++)
for (int j=starty*16;j<(starty*16+16);j++)
{
btVector4 rgba(0,0,0,1);
rgba[0] = (sFontData[i*3+255*256*3-(256*j)*3])/255.f;
//rgba[0] += (sFontData[(i+1)*3+255*256*3-(256*j)*3])/255.*0.25f;
//rgba[0] += (sFontData[(i)*3+255*256*3-(256*j+1)*3])/255.*0.25f;
//rgba[0] += (sFontData[(i+1)*3+255*256*3-(256*j+1)*3])/255.*0.25;
//if (rgba[0]!=0.f)
{
rgba[1]=rgba[0];
rgba[2]=rgba[0];
rgba[3]=1.f;
//raytracePicture->setPixel(x,y,rgba);
raytracePicture->addPixel(x,y,rgba);
}
y++;
}
x++;
}
}
}
//raytracePicture->grapicalPrintf("CCD RAYTRACER",sFontData);
char buffer[256];
sprintf(buffer,"%d rays",screenWidth*screenHeight*numObjects);
//sprintf(buffer,"Toggle",screenWidth*screenHeight*numObjects);
//sprintf(buffer,"TEST",screenWidth*screenHeight*numObjects);
//raytracePicture->grapicalPrintf(buffer,sFontData,0,10);//&BMF_font_helv10,0,10);
raytracePicture->grapicalPrintf(buffer,sFontData,0,0);//&BMF_font_helv10,0,10);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0);
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity(); // reset The Modelview Matrix
glTranslatef(0.0f,0.0f,-3.1f); // Move Into The Screen 5 Units
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,glTextureId );
const unsigned char *ptr = raytracePicture->getBuffer();
glTexImage2D(GL_TEXTURE_2D,
0,
GL_RGBA,
raytracePicture->getWidth(),raytracePicture->getHeight(),
0,
GL_RGBA,
GL_UNSIGNED_BYTE,
ptr);
glEnable (GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f (1,1,1,1); // alpha=0.5=half visible
glBegin(GL_QUADS);
glTexCoord2f(0.0f, 0.0f);
glVertex2f(-1,1);
glTexCoord2f(1.0f, 0.0f);
glVertex2f(1,1);
glTexCoord2f(1.0f, 1.0f);
glVertex2f(1,-1);
glTexCoord2f(0.0f, 1.0f);
glVertex2f(-1,-1);
glEnd();
glMatrixMode(GL_MODELVIEW);
glPopMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glDisable(GL_TEXTURE_2D);
glDisable(GL_DEPTH_TEST);
GL_ShapeDrawer::drawCoordSystem();
{
for (int i=0;i<numObjects;i++)
{
btVector3 aabbMin,aabbMax;
shapePtr[i]->getAabb(transforms[i],aabbMin,aabbMax);
}
}
glPushMatrix();
glPopMatrix();
pitch += 0.005f;
yaw += 0.01f;
m_azi += 1.f;
glFlush();
glutSwapBuffers();
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA, const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut)
{
gActualSATPairTests++;
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int numFacesA = hullA.m_faces.size();
// Test normals from hullA
for(int i=0;i<numFacesA;i++)
{
const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
faceANormalWS*=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
btVector3 wA, wB;
if(!TestSepAxis( hullA, hullB, transA, transB, faceANormalWS, d, wA, wB))
return false;
if(d<dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.size();
// Test normals from hullB
for(int i=0;i<numFacesB;i++)
{
const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
WorldNormal *=-1.f;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
btVector3 wA, wB;
if(!TestSepAxis(hullA, hullB, transA, transB, WorldNormal, d,wA, wB))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
btVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
int edgeA=-1;
int edgeB=-1;
btVector3 worldEdgeA;
btVector3 worldEdgeB;
btVector3 witnessPointA, witnessPointB;
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
{
const btVector3 edge0 = hullA.m_uniqueEdges[e0];
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
{
const btVector3 edge1 = hullB.m_uniqueEdges[e1];
const btVector3 WorldEdge1 = transB.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
curEdgeEdge++;
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
Cross *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar dist;
btVector3 wA, wB;
if(!TestSepAxis( hullA, hullB, transA, transB, Cross, dist, wA, wB))
return false;
if(dist<dmin)
{
dmin = dist;
sep = Cross;
edgeA=e0;
edgeB=e1;
worldEdgeA = WorldEdge0;
worldEdgeB = WorldEdge1;
witnessPointA=wA;
witnessPointB=wB;
}
}
}
}
if (edgeA>=0&&edgeB>=0)
{
// printf("edge-edge\n");
//add an edge-edge contact
btVector3 ptsVector;
btVector3 offsetA;
btVector3 offsetB;
btScalar tA;
btScalar tB;
btVector3 translation = witnessPointB-witnessPointA;
btVector3 dirA = worldEdgeA;
btVector3 dirB = worldEdgeB;
btScalar hlenB = 1e30f;
btScalar hlenA = 1e30f;
btSegmentsClosestPoints(ptsVector, offsetA, offsetB, tA, tB,
translation,
dirA, hlenA,
dirB, hlenB);
btScalar nlSqrt = ptsVector.length2();
if (nlSqrt>SIMD_EPSILON)
{
btScalar nl = btSqrt(nlSqrt);
ptsVector *= 1.f/nl;
if (ptsVector.dot(DeltaC2)<0.f)
{
ptsVector*=-1.f;
}
btVector3 ptOnB = witnessPointB + offsetB;
btScalar distance = nl;
resultOut.addContactPoint(ptsVector, ptOnB,-distance);
}
}
if((DeltaC2.dot(sep))<0.0f)
sep = -sep;
return true;
}
// Returns the portion of 'direction' that is parallel to 'normal'
btVector3 CCharacterController::ParallelComponent(const btVector3& direction, const btVector3& normal)
{
btScalar magnitude = direction.dot(normal);
return normal * magnitude;
}
bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep)
{
gActualSATPairTests++;
//#ifdef TEST_INTERNAL_OBJECTS
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
//#endif
btScalar dmin = FLT_MAX;
int curPlaneTests=0;
int numFacesA = hullA.m_faces.size();
// Test normals from hullA
for(int i=0;i<numFacesA;i++)
{
const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
const btVector3 faceANormalWS = transA.getBasis() * Normal;
if (DeltaC2.dot(faceANormalWS)<0)
continue;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d))
return false;
if(d<dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.size();
// Test normals from hullB
for(int i=0;i<numFacesB;i++)
{
const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
if (DeltaC2.dot(WorldNormal)<0)
continue;
curPlaneTests++;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar d;
if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d))
return false;
if(d<dmin)
{
dmin = d;
sep = WorldNormal;
}
}
btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
int curEdgeEdge = 0;
// Test edges
for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++)
{
const btVector3 edge0 = hullA.m_uniqueEdges[e0];
const btVector3 WorldEdge0 = transA.getBasis() * edge0;
for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++)
{
const btVector3 edge1 = hullB.m_uniqueEdges[e1];
const btVector3 WorldEdge1 = transB.getBasis() * edge1;
btVector3 Cross = WorldEdge0.cross(WorldEdge1);
curEdgeEdge++;
if(!IsAlmostZero(Cross))
{
Cross = Cross.normalize();
if (DeltaC2.dot(Cross)<0)
continue;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin))
continue;
gActualNbTests++;
#endif
btScalar dist;
if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist))
return false;
if(dist<dmin)
{
dmin = dist;
sep = Cross;
}
}
}
}
const btVector3 deltaC = transB.getOrigin() - transA.getOrigin();
if((deltaC.dot(sep))>0.0f)
sep = -sep;
return true;
}