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];
btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly)
{
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];
for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
// compute three elements of right hand side
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 * normalErp;//??
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())
{
#ifdef _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = getEnableAngularMotor();
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 = false;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
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)
#ifdef _BT_USE_CENTER_LIMIT_
btScalar bounce = m_limit.getRelaxationFactor();
#else
btScalar bounce = m_relaxationFactor;
#endif
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;
}
}
}
}
#ifdef _BT_USE_CENTER_LIMIT_
info->m_constraintError[srow] *= m_limit.getBiasFactor();
#else
info->m_constraintError[srow] *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}
int btGeneric6DofConstraint::get_limit_motor_info2(
btRotationalLimitMotor * limot,
btRigidBody * body0, btRigidBody * body1,
btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
{
int srow = row * info->rowskip;
int powered = limot->m_enableMotor;
int limit = limot->m_currentLimit;
if (powered || limit)
{ // if the joint is powered, or has joint limits, add in the extra row
btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
if(rotational)
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
if((!rotational))
{
btVector3 ltd; // Linear Torque Decoupling vector
btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
ltd = -c.cross(ax1);
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
info->m_constraintError[srow] = btScalar(0.f);
if (powered)
{
info->cfm[srow] = 0.0f;
if(!limit)
{
btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
limot->m_loLimit,
limot->m_hiLimit,
tag_vel,
info->fps * info->erp);
info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
}
}
if(limit)
{
btScalar k = info->fps * limot->m_ERP;
if(!rotational)
{
info->m_constraintError[srow] += k * limot->m_currentLimitError;
}
else
{
info->m_constraintError[srow] += -k * limot->m_currentLimitError;
}
info->cfm[srow] = 0.0f;
if (limot->m_loLimit == limot->m_hiLimit)
{ // limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{
if (limit == 1)
{
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// deal with bounce
if (limot->m_bounce > 0)
{
// calculate joint velocity
btScalar vel;
if (rotational)
{
vel = body0->getAngularVelocity().dot(ax1);
if (body1)
vel -= body1->getAngularVelocity().dot(ax1);
}
else
{
vel = body0->getLinearVelocity().dot(ax1);
if (body1)
vel -= body1->getLinearVelocity().dot(ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
if (vel < 0)
{
btScalar newc = -limot->m_bounce* vel;
if (newc > info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
else
{
if (vel > 0)
{
btScalar newc = -limot->m_bounce * vel;
if (newc < info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const
{
//moving concave objects not supported
inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}
void btOptimizedBvh::updateBvhNodes(btStridingMeshInterface* meshInterface,int firstNode,int endNode,int index)
{
(void)index;
btAssert(m_useQuantization);
int curNodeSubPart=-1;
//get access info to trianglemesh data
const unsigned char *vertexbase = 0;
int numverts = 0;
PHY_ScalarType type = PHY_INTEGER;
int stride = 0;
const unsigned char *indexbase = 0;
int indexstride = 0;
int numfaces = 0;
PHY_ScalarType indicestype = PHY_INTEGER;
btVector3 triangleVerts[3];
btVector3 aabbMin,aabbMax;
const btVector3& meshScaling = meshInterface->getScaling();
int i;
for (i=endNode-1; i>=firstNode; i--)
{
btQuantizedBvhNode& curNode = m_quantizedContiguousNodes[i];
if (curNode.isLeafNode())
{
//recalc aabb from triangle data
int nodeSubPart = curNode.getPartId();
int nodeTriangleIndex = curNode.getTriangleIndex();
if (nodeSubPart != curNodeSubPart)
{
if (curNodeSubPart >= 0)
meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,nodeSubPart);
curNodeSubPart = nodeSubPart;
btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT);
}
//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
unsigned int* gfxbase = (unsigned int*)(indexbase+nodeTriangleIndex*indexstride);
for (int j=2; j>=0; j--)
{
int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
if (type == PHY_FLOAT)
{
float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
triangleVerts[j] = btVector3(
graphicsbase[0]*meshScaling.getX(),
graphicsbase[1]*meshScaling.getY(),
graphicsbase[2]*meshScaling.getZ());
}
else
{
double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
}
}
aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
aabbMin.setMin(triangleVerts[0]);
aabbMax.setMax(triangleVerts[0]);
aabbMin.setMin(triangleVerts[1]);
aabbMax.setMax(triangleVerts[1]);
aabbMin.setMin(triangleVerts[2]);
aabbMax.setMax(triangleVerts[2]);
quantize(&curNode.m_quantizedAabbMin[0],aabbMin,0);
quantize(&curNode.m_quantizedAabbMax[0],aabbMax,1);
} else
{
//combine aabb from both children
btQuantizedBvhNode* leftChildNode = &m_quantizedContiguousNodes[i+1];
btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? &m_quantizedContiguousNodes[i+2] :
&m_quantizedContiguousNodes[i+1+leftChildNode->getEscapeIndex()];
{
for (int i=0; i<3; i++)
{
curNode.m_quantizedAabbMin[i] = leftChildNode->m_quantizedAabbMin[i];
if (curNode.m_quantizedAabbMin[i]>rightChildNode->m_quantizedAabbMin[i])
curNode.m_quantizedAabbMin[i]=rightChildNode->m_quantizedAabbMin[i];
curNode.m_quantizedAabbMax[i] = leftChildNode->m_quantizedAabbMax[i];
if (curNode.m_quantizedAabbMax[i] < rightChildNode->m_quantizedAabbMax[i])
curNode.m_quantizedAabbMax[i] = rightChildNode->m_quantizedAabbMax[i];
}
}
}
}
if (curNodeSubPart >= 0)
meshInterface->unLockReadOnlyVertexBase(curNodeSubPart);
}
btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
{
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_ERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(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);
bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
return clippedMotorImpulse;
}
btRigidBody* Physics::addRigidSphere(Ogre::Entity* entity, Ogre::SceneNode* node,
btScalar mass, btScalar rest, btVector3 localInertia, btVector3 origin, btQuaternion *rotation) {
Ogre::Vector3 s = entity->getBoundingBox().getHalfSize();
btCollisionShape *sphereShape = new btSphereShape( btScalar(s[0]) );
addRigidBody(entity, node, sphereShape, mass, rest, localInertia, origin, rotation);
};
SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans)
: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), m_worldTrans(trans) ,m_maxDot(btScalar(-1e30))
{
m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis();
}
bool Skeleton_Builder::BuildTail(float tail_width, float tail_height, float torso_height)
{
Bone* UTail = new Bone();
Bone* MTail = new Bone();
Bone* LTail = new Bone();
Bone* LLTail = new Bone();
tail_height /= 4;
Ogre::Quaternion neckrotation;
neckrotation.FromAngleAxis(Ogre::Radian(Ogre::Degree(mTailIncline)), Ogre::Vector3::UNIT_X);
btTransform t;
btVector3 min, max;
mHipNode->GetCollider()->getAabb(t, min, max);
mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -(torso_height /2), mDepth / 2), neckrotation, *mHipNode->GetNode());
if (!mBuilder->BuildBone(*LLTail, Bone::BoneType::Tail))
return false;
mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height , 0), neckrotation, *LLTail->GetNode());
if (!mBuilder->BuildBone(*LTail, Bone::BoneType::Tail))
return false;
mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height, 0), neckrotation, *LTail->GetNode());
if (!mBuilder->BuildBone(*MTail, Bone::BoneType::Tail))
return false;
mBuilder->SetDimensions(tail_width, tail_height, mDepth / 2);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -tail_height, 0), neckrotation, *MTail->GetNode());
if (!mBuilder->BuildBone(*UTail, Bone::BoneType::Tail))
return false;
if (mHasMuscle)
{
mMuscleBuilder->CreateMuscle(mHipNode, LLTail, mMuscles);
mMuscleBuilder->CreateMuscle(LLTail, LTail, mMuscles);
mMuscleBuilder->CreateMuscle(LTail, MTail, mMuscles);
mMuscleBuilder->CreateMuscle(MTail, UTail, mMuscles);
}
//add joints
btTransform localA, localB;
btVector3 JointPos;
JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (torso_height / 2), 0));
SetJointTransform(localA, localB, JointPos, mHipNode, LLTail, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* shoulderNeckConst = new btConeTwistConstraint(*mHipNode->GetRigidBody(), *LLTail->GetRigidBody(), localA, localB);
shoulderNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4),btScalar(M_PI_4 / 4));
JointPos = Utils::OgreBTVector(LLTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0));
SetJointTransform(localA, localB, JointPos, LLTail, LTail, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* LNeckConst = new btConeTwistConstraint(*LLTail->GetRigidBody(), *LTail->GetRigidBody(), localA, localB);
LNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
JointPos = Utils::OgreBTVector(LTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0));
SetJointTransform(localA, localB, JointPos, LTail, MTail, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* MNeckConst = new btConeTwistConstraint(*LTail->GetRigidBody(), *MTail->GetRigidBody(), localA, localB);
MNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
JointPos = Utils::OgreBTVector(MTail->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (tail_height), 0));
SetJointTransform(localA, localB, JointPos, MTail, UTail, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* HNeckConst = new btConeTwistConstraint(*MTail->GetRigidBody(), *UTail->GetRigidBody(), localA, localB);
HNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
mConstraints.push_back(shoulderNeckConst);
mConstraints.push_back(LNeckConst);
mConstraints.push_back(MNeckConst);
mConstraints.push_back(HNeckConst);
return true;
}
bool btGjkConvexCast::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
m_simplexSolver->reset();
/// compute linear velocity for this interval, to interpolate
//assume no rotation/angular velocity, assert here?
btVector3 linVelA, linVelB;
linVelA = toA.getOrigin()-fromA.getOrigin();
linVelB = toB.getOrigin()-fromB.getOrigin();
btScalar radius = btScalar(0.001);
btScalar lambda = btScalar(0.);
btVector3 v(1,0,0);
int maxIter = MAX_ITERATIONS;
btVector3 n;
n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
bool hasResult = false;
btVector3 c;
btVector3 r = (linVelA-linVelB);
btScalar lastLambda = lambda;
//btScalar epsilon = btScalar(0.001);
int numIter = 0;
//first solution, using GJK
btTransform identityTrans;
identityTrans.setIdentity();
// result.drawCoordSystem(sphereTr);
btPointCollector pointCollector;
btGjkPairDetector gjk(m_convexA, m_convexB, m_simplexSolver,0);//m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.getClosestPoints(input, pointCollector,0);
hasResult = pointCollector.m_hasResult;
c = pointCollector.m_pointInWorld;
if (hasResult)
{
btScalar dist;
dist = pointCollector.m_distance;
n = pointCollector.m_normalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
}
btScalar dLambda = btScalar(0.);
btScalar projectedLinearVelocity = r.dot(n);
dLambda = dist / (projectedLinearVelocity);
lambda = lambda - dLambda;
if (lambda > btScalar(1.))
return false;
if (lambda < btScalar(0.))
return false;
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
break;
}
lastLambda = lambda;
//interpolate to next lambda
result.DebugDraw( lambda );
input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(), toA.getOrigin(), lambda);
input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda);
gjk.getClosestPoints(input, pointCollector,0);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < btScalar(0.))
{
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal=n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
} else
{
//??
return false;
}
}
//is n normalized?
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (n.dot(r)>=-result.m_allowedPenetration)
return false;
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
}
bool Skeleton_Builder::BuildSkeleton(Skeleton& newSkel, Ogre::Vector3 pos)
{
if (mLeg == LegType::None || mHeight <= 10) // check to ensure all attribs required
return false;
float neck_height;
float torso_height;
float Leg_height;
float tail_height;
//create base node for skeleton
mBase = mSceneMgr->getRootSceneNode()->createChildSceneNode(pos);
// set block area heights
// if longneck neck/legs ratio = 2:1
// if shortneck neck/legs ratio = 1:2
// if upright torso 40%
// else torso 25%
if (IsUpright())
{
auto onepercent = mHeight / 100;
Leg_height = onepercent * 40;
if (IsLongNeck())
{
neck_height = onepercent * 40;
torso_height = onepercent * 20;
}
else
{
neck_height = onepercent * 20;
torso_height = onepercent * 40;
}
}
else
{
auto onepercent = mHeight / 100;
torso_height = onepercent * 25;
if (IsLongNeck())
{
neck_height = onepercent * 50;
Leg_height = onepercent * 25;
}
else
{
neck_height = onepercent * 25;
Leg_height = onepercent * 50;
}
}
if (IsLongTail())
{
auto onepercent = mHeight / 100;
tail_height = onepercent * 50;
}
else
{
auto onepercent = mHeight / 100;
tail_height = onepercent * 25;
}
//set block area widths
float neck_width;
float torso_width;
float Leg_width;
float Arm_width;
auto onepercentW = mWidth / 100;
// arms take 35% of width i.e 17.5% each side if needed
torso_width = onepercentW * 65;
Leg_width = onepercentW * 25; // ensures gaps between legs like realistic bipeds
Arm_width = onepercentW * 17.5f;
neck_width = onepercentW * 35;
//build the torso here
Bone* CentreTorso = new Bone();
if (IsUpright())
{
Bone* TopTorso = new Bone();
Bone* BotTorso = new Bone();
//build 3 torso pieces
torso_height /= 3;
//build centre piece
mBuilder->SetDimensions(torso_width, torso_height, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3::ZERO, Ogre::Quaternion::IDENTITY,*mBase);
if (!mBuilder->BuildBone(*CentreTorso,Bone::BoneType::Torso))
return false;
//build bot piece
mBuilder->SetDimensions(torso_width, torso_height, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -(torso_height), 0), Ogre::Quaternion::IDENTITY, *CentreTorso->GetNode());
if (!mBuilder->BuildBone(*BotTorso,Bone::BoneType::Hip))
return false;
//build top piece
mBuilder->SetDimensions(torso_width, torso_height, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3(0, (torso_height), 0), Ogre::Quaternion::IDENTITY, *CentreTorso->GetNode());
if (!mBuilder->BuildBone(*TopTorso, Bone::BoneType::Shoulder))
return false;
mShouldernode = TopTorso;
mHipNode = BotTorso;
mBones.push_back(CentreTorso);
mBones.push_back(TopTorso);
mBones.push_back(BotTorso);
//add muscles
mMuscleBuilder->CreateMuscle(CentreTorso, TopTorso, mMuscles);
mMuscleBuilder->CreateMuscle(CentreTorso, BotTorso, mMuscles);
btTransform TransA;
btTransform TransB;
btVector3 JointPos; // world position of transform
/*add constraints*/
// JOINT POS IS THE POSITION ON THE EDGE OF THE BONE ATTACH I.E BONE POS + EDGE OF BONE IN DIRECTION TO NEXT
JointPos = Utils::OgreBTVector(CentreTorso->GetNode()->_getDerivedPosition() + (torso_height / 2));
SetJointTransform(TransA, TransB, JointPos, CentreTorso, TopTorso, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* TopSpine = new btConeTwistConstraint(*CentreTorso->GetRigidBody(), *TopTorso->GetRigidBody(), TransA, TransB);
TopSpine->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
JointPos = Utils::OgreBTVector(CentreTorso->GetNode()->_getDerivedPosition() - (torso_height / 2));
SetJointTransform(TransA, TransB, JointPos, CentreTorso, BotTorso, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* BotSpine = new btConeTwistConstraint(*CentreTorso->GetRigidBody(), *BotTorso->GetRigidBody(), TransA, TransB);
BotSpine->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
mConstraints.push_back(TopSpine);
mConstraints.push_back(BotSpine);
}
else
{
//Bone* CentreTorso = new Bone();
//build centre piece
mBuilder->SetDimensions(torso_width, torso_height, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3::ZERO, Ogre::Quaternion::IDENTITY, *mBase);
if (!mBuilder->BuildBone(*CentreTorso,Bone::BoneType::HipShoulder))
return false;
mBones.push_back(CentreTorso);
mShouldernode = CentreTorso;
mHipNode = CentreTorso;
}
if (mFixed)
CentreTorso->GetRigidBody()->setMassProps(0, btVector3(0, 0, 0));
//if (!BuildArm(Arm_width, Leg_height, torso_width) || !BuildNeck(neck_width, neck_height, torso_height) || !BuildLeg(Leg_width, Leg_height, torso_width, torso_height))
// return false;
if (!mHidearms)
if (!BuildArm(Arm_width, torso_height, torso_width))
return false;
if (!mHideneck)
if (!BuildNeck(neck_width, neck_height, torso_height))
return false;
if (!mHidelegs)
if (!BuildLeg(Leg_width, Leg_height, torso_width, torso_height))
return false;
if (IsShortTail() || IsLongTail())
if(!BuildTail(neck_width,tail_height, torso_height))
return false;
newSkel = Skeleton(mBones,mMuscles, mConstraints, mBase, mWorld);
ClearData();
return true;
}
bool Skeleton_Builder::BuildLeg(float leg_width, float leg_height, float torso_width, float torso_height)
{
auto halfgap = (torso_width / 2) - leg_width;
auto legnodepos = halfgap + leg_width / 2;
Bone* LUpperLeg = new Bone();
Bone* LLowerLeg = new Bone();
Bone* LFoot = new Bone();
Bone* RUpperLeg = new Bone();
Bone* RLowerLeg = new Bone();
Bone* RFoot = new Bone();
if (mLeg == LegType::Uninverted)
{
mBuilder->SetDimensions(leg_width, leg_height / 2, mDepth *0.75f);
mBuilder->SetRelativePosition(Ogre::Vector3(legnodepos, -((torso_height / 2) + (leg_height / 4)), 0), Ogre::Quaternion::IDENTITY, *mHipNode->GetNode());
if (!mBuilder->BuildBone(*LUpperLeg,Bone::BoneType::LUpperLeg))
return false;
mBuilder->SetDimensions(leg_width, leg_height / 2, mDepth *0.75f);
mBuilder->SetRelativePosition(Ogre::Vector3(-legnodepos, -((torso_height / 2) + (leg_height / 4)), 0), Ogre::Quaternion::IDENTITY, *mHipNode->GetNode());
if (!mBuilder->BuildBone(*RUpperLeg, Bone::BoneType::RUpperLeg))
return false;
mBuilder->SetDimensions(leg_width, leg_height / 2, mDepth *0.75f);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *LUpperLeg->GetNode());
if (!mBuilder->BuildBone(*LLowerLeg, Bone::BoneType::LowerLeg))
return false;
mBuilder->SetDimensions(leg_width, leg_height / 2, mDepth *0.75f);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *RUpperLeg->GetNode());
if (!mBuilder->BuildBone(*RLowerLeg, Bone::BoneType::LowerLeg))
return false;
// foot generation
mBuilder->SetDimensions(leg_width*1.5f, leg_height / 4, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *LLowerLeg->GetNode());
if (!mBuilder->BuildBone(*LFoot,Bone::BoneType::Foot))
return false;
mBuilder->SetDimensions(leg_width*1.5f, leg_height / 4, mDepth);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *RLowerLeg->GetNode());
if (!mBuilder->BuildBone(*RFoot, Bone::BoneType::Foot))
return false;
}
else
{
//mBuilder->SetDimensions(leg_width, leg_height / 2);
//mBuilder->SetRelativePosition(Ogre::Vector3(legnodepos, 0, 0), Ogre::Quaternion::IDENTITY, *mHipNode->GetNode());
//if (!mBuilder->BuildBone(*LUpperLeg))
// return false;
//mBuilder->SetDimensions(leg_width, leg_height / 2);
//mBuilder->SetRelativePosition(Ogre::Vector3(0, leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *LUpperLeg->GetNode());
//if (!mBuilder->BuildBone(*LLowerLeg))
// return false;
// foot generation
//mBuilder->SetDimensions(leg_width, leg_height / 2);
//mBuilder->SetRelativePosition(Ogre::Vector3(legnodepos, 0, 0), Ogre::Quaternion::IDENTITY, Hip->GetNode());
//if (!mBuilder->BuildBone(LUpperLeg))
// return false;
//mBuilder->SetDimensions(leg_width, leg_height / 2);
//mBuilder->SetRelativePosition(Ogre::Vector3(-legnodepos, 0, 0), Ogre::Quaternion::IDENTITY, *mHipNode->GetNode());
//if (!mBuilder->BuildBone(*RUpperLeg))
// return false;
//mBuilder->SetDimensions(leg_width, leg_height / 2);
//mBuilder->SetRelativePosition(Ogre::Vector3(0, leg_height / 2, 0), Ogre::Quaternion::IDENTITY, *RUpperLeg->GetNode());
//if (!mBuilder->BuildBone(*RLowerLeg))
// return false;
// foot generation
//mBuilder->SetDimensions(leg_width, leg_height / 2);
//mBuilder->SetRelativePosition(Ogre::Vector3(legnodepos, 0, 0), Ogre::Quaternion::IDENTITY, Hip->GetNode());
//if (!mBuilder->BuildBone(LUpperLeg))
// return false;
}
mBones.push_back(LUpperLeg);
mBones.push_back(RUpperLeg);
mBones.push_back(LLowerLeg);
mBones.push_back(RLowerLeg);
mBones.push_back(LFoot);
mBones.push_back(RFoot);
if (mHasMuscle)
{
mMuscleBuilder->CreateMuscle(mHipNode, LUpperLeg, mMuscles);
mMuscleBuilder->CreateMuscle(LUpperLeg, LLowerLeg, mMuscles);
mMuscleBuilder->CreateMuscle(LLowerLeg, LFoot, mMuscles);
mMuscleBuilder->CreateMuscle(mHipNode, RUpperLeg, mMuscles);
mMuscleBuilder->CreateMuscle(RUpperLeg, RLowerLeg, mMuscles);
mMuscleBuilder->CreateMuscle(RLowerLeg, RFoot, mMuscles);
}
//add joints
Ogre::Vector3 offset;
btTransform localA, localB;
btVector3 JointPos;
JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(legnodepos, -(torso_height / 2), 0));
SetJointTransform(localA, localB, JointPos, mHipNode, LUpperLeg, btVector3(0, btScalar(M_PI_2), 0));
btConeTwistConstraint* LhipConst = new btConeTwistConstraint(*mHipNode->GetRigidBody(), *LUpperLeg->GetRigidBody(), localA, localB);
LhipConst->setLimit(btScalar(M_PI_4), btScalar(M_PI_4), 0);
JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(-legnodepos, -(torso_height / 2), 0));
SetJointTransform(localA, localB, JointPos, mHipNode, RUpperLeg, btVector3(0, 0, btScalar(M_PI_4)));
btConeTwistConstraint* RhipConst = new btConeTwistConstraint(*mHipNode->GetRigidBody(), *RUpperLeg->GetRigidBody(), localA, localB);
RhipConst->setLimit(btScalar(M_PI_4), btScalar(M_PI_4), 0);
JointPos = Utils::OgreBTVector(LUpperLeg->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, -(leg_height / 2), 0));
SetJointTransform(localA, localB, JointPos, LUpperLeg, LLowerLeg, btVector3(0, btScalar(M_PI_2), 0));
btHingeConstraint* LLegConst = new btHingeConstraint(*LUpperLeg->GetRigidBody(), *LLowerLeg->GetRigidBody(), localA, localB);
LLegConst->setLimit(0, btScalar(M_PI_2));
JointPos = Utils::OgreBTVector(RUpperLeg->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, -(leg_height / 2), 0));
SetJointTransform(localA, localB, JointPos, RUpperLeg, RLowerLeg, btVector3(0, btScalar(M_PI_2), 0));
btHingeConstraint* RLegConst = new btHingeConstraint(*RUpperLeg->GetRigidBody(), *RLowerLeg->GetRigidBody(), localA, localB);
RLegConst->setLimit(0, btScalar(M_PI_2));
JointPos = Utils::OgreBTVector(LLowerLeg->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, -(leg_height / 2), 0));
SetJointTransform(localA, localB, JointPos, LLowerLeg, LFoot, btVector3(0, btScalar(M_PI_2), 0));
btHingeConstraint* LFootConst = new btHingeConstraint(*LLowerLeg->GetRigidBody(), *LFoot->GetRigidBody(), localA, localB);
LFootConst->setLimit(0, btScalar(M_PI_2));
JointPos = Utils::OgreBTVector(RLowerLeg->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, -(leg_height / 2), 0));
SetJointTransform(localA, localB, JointPos, RLowerLeg, RFoot, btVector3(0, btScalar(M_PI_2), 0));
btHingeConstraint* RFootConst = new btHingeConstraint(*RLowerLeg->GetRigidBody(), *RFoot->GetRigidBody(), localA, localB);
RFootConst->setLimit(0, btScalar(M_PI_2));
mConstraints.push_back(LhipConst);
mConstraints.push_back(RhipConst);
mConstraints.push_back(LLegConst);
mConstraints.push_back(RLegConst);
mConstraints.push_back(LFootConst);
mConstraints.push_back(RFootConst);
return true;
}
bool Skeleton_Builder::BuildNeck(float neck_width, float neck_height, float torso_height)
{
Bone* UNeck = new Bone();
Bone* MNeck = new Bone();
Bone* LNeck = new Bone();
Bone* LLNeck = new Bone();
neck_height /= 4;
mBuilder->SetDimensions(neck_width, neck_height, mDepth * 0.5f);
Ogre::Quaternion neckrotation;
neckrotation.FromAngleAxis(Ogre::Radian(Ogre::Degree(mNeckIncline)), Ogre::Vector3::UNIT_X);
mBuilder->SetRelativePosition(Ogre::Vector3(0, neck_height, 0), neckrotation, *mShouldernode->GetNode());
if (!mBuilder->BuildBone(*LLNeck,Bone::BoneType::Neck))
return false;
mBuilder->SetDimensions(neck_width, neck_height, mDepth * 0.5f);
mBuilder->SetRelativePosition(Ogre::Vector3(0, neck_height, 0), neckrotation, *LLNeck->GetNode());
if (!mBuilder->BuildBone(*LNeck, Bone::BoneType::Neck))
return false;
mBuilder->SetDimensions(neck_width, neck_height, mDepth * 0.5f);
mBuilder->SetRelativePosition(Ogre::Vector3(0, neck_height, 0), neckrotation, *LNeck->GetNode());
if (!mBuilder->BuildBone(*MNeck, Bone::BoneType::Neck))
return false;
mBuilder->SetDimensions(neck_width, neck_height, mDepth * 0.5f);
mBuilder->SetRelativePosition(Ogre::Vector3(0, neck_height, 0), neckrotation, *MNeck->GetNode());
if (!mBuilder->BuildBone(*UNeck, Bone::BoneType::Neck))
return false;
if (mHasMuscle)
{
mMuscleBuilder->CreateMuscle(mShouldernode, LLNeck, mMuscles);
mMuscleBuilder->CreateMuscle(LLNeck, LNeck, mMuscles);
mMuscleBuilder->CreateMuscle(LNeck, MNeck, mMuscles);
mMuscleBuilder->CreateMuscle(MNeck, UNeck, mMuscles);
}
//add joints
btTransform localA, localB;
btVector3 JointPos;
JointPos = Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (torso_height / 2), 0));
SetJointTransform(localA, localB, JointPos, mShouldernode, LLNeck, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* shoulderNeckConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *LLNeck->GetRigidBody(), localA, localB);
shoulderNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
JointPos = Utils::OgreBTVector(LLNeck->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (neck_height / 2), 0));
SetJointTransform(localA, localB, JointPos, LLNeck, LNeck, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* LNeckConst = new btConeTwistConstraint(*LLNeck->GetRigidBody(), *LNeck->GetRigidBody(), localA, localB);
LNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (neck_height / 2), 0));
SetJointTransform(localA, localB, JointPos, LNeck, MNeck, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* MNeckConst = new btConeTwistConstraint(*LNeck->GetRigidBody(), *MNeck->GetRigidBody(), localA, localB);
MNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4));
JointPos = Utils::OgreBTVector(mHipNode->GetNode()->_getDerivedPosition() + Ogre::Vector3(0, (neck_height / 2), 0));
SetJointTransform(localA, localB, JointPos, MNeck, UNeck, btVector3(0, btScalar(-M_PI_2), 0));
btConeTwistConstraint* HNeckConst = new btConeTwistConstraint(*MNeck->GetRigidBody(), *UNeck->GetRigidBody(), localA, localB);
HNeckConst->setLimit(btScalar(M_PI_4 / 4), btScalar(M_PI_4 / 4),btScalar(M_PI_4 / 4));
mConstraints.push_back(shoulderNeckConst);
mConstraints.push_back(LNeckConst);
mConstraints.push_back(MNeckConst);
mConstraints.push_back(HNeckConst);
return true;
}
bool Skeleton_Builder::BuildArm(float arm_Width, float arm_height, float torso_width)
{
// 3 blocks no hands
// only height/ width changes
//if (mArm == ArmType::ShortArms)
//{
// //half height of legs
// Bone* LUpperArm = new Bone();
// Bone* LLowerArm = new Bone();
// //build shoulder piece
// torso_width /= 2;
// mBuilder->SetDimensions(arm_Width, arm_height/2);
// mBuilder->SetRelativePosition(Ogre::Vector3(torso_width, 0, 0), Ogre::Quaternion::IDENTITY, *mShouldernode->GetNode());
// if (!mBuilder->BuildBone(*LUpperArm))
// return false;
//
// mBuilder->SetDimensions(arm_Width, arm_height / 2);
// mBuilder->SetRelativePosition(Ogre::Vector3(0, arm_height / 2, 0), Ogre::Quaternion::IDENTITY, *LUpperArm->GetNode());
// if (!mBuilder->BuildBone(*LLowerArm))
// return false;
// mBones.push_back(LUpperArm);
// mBones.push_back(LLowerArm);
// //Bone* RUpperArm = new Bone();
// //Bone* RLowerArm = new Bone();
// //mBuilder->SetDimensions(arm_Width, arm_height / 2);
// //mBuilder->SetRelativePosition(Ogre::Vector3(-torso_width, 0, 0), Ogre::Quaternion::IDENTITY, *mShouldernode->GetNode());
// //if (!mBuilder->BuildBone(*RUpperArm))
// // return false;
// //
// //mBuilder->SetDimensions(arm_Width, arm_height / 2);
// //mBuilder->SetRelativePosition(Ogre::Vector3(0, arm_height / 2, 0), Ogre::Quaternion::IDENTITY, *RUpperArm->GetNode());
// //if (!mBuilder->BuildBone(*RLowerArm))
// // return false;
// //mBones.push_back(RUpperArm);
// //mBones.push_back(RLowerArm);
// //add joints
// btTransform localA, localB;
// Ogre::Vector3 offset;
// offset = Ogre::Vector3(torso_width, 0, 0);
// //auto vec = mShouldernode->GetNode()->_getDerivedPosition();
// //vec += offset;
// SetJointTransform(localA, localB, Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() + offset), mShouldernode, LUpperArm);
// btConeTwistConstraint* LshoulderConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *LUpperArm->GetRigidBody(), localA, localB);
// LshoulderConst->setLimit(M_PI_2, M_PI_2, 0);
// //SetJointTransform(localA, localB, Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() - offset), mShouldernode, RUpperArm);
// //btConeTwistConstraint* RshoulderConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *RUpperArm->GetRigidBody(), localA, localB);
// //RshoulderConst->setLimit(M_PI_2, M_PI_2, 0);
// offset = Ogre::Vector3(0, arm_height/4, 0);
// SetJointTransform(localA, localB, Utils::OgreBTVector(LUpperArm->GetNode()->_getDerivedPosition() - offset), LUpperArm, LLowerArm);
// btHingeConstraint* LArmConst = new btHingeConstraint(*LUpperArm->GetRigidBody(), *LLowerArm->GetRigidBody(), localA, localB);
// //SetJointTransform(localA, localB, Utils::OgreBTVector(RUpperArm->GetNode()->_getDerivedPosition() - offset), RUpperArm, RLowerArm);
// //btHingeConstraint* RArmConst = new btHingeConstraint(*RUpperArm->GetRigidBody(), *RLowerArm->GetRigidBody(), localA, localB);
// mConstraints.push_back(LshoulderConst);
// mConstraints.push_back(LArmConst);
// //mConstraints.push_back(RshoulderConst);
// //mConstraints.push_back(RArmConst);
//
//}
//else if (mArm == ArmType::LongArms)
{
//half height of legs
Bone* LUpperArm = new Bone();
Bone* RUpperArm = new Bone();
Bone* LLowerArm = new Bone();
Bone* RLowerArm = new Bone();
//build shoulder piece
//torso_width /= 2;
mBuilder->SetDimensions(arm_Width, arm_height, mDepth * 0.5f);
mBuilder->SetRelativePosition(Ogre::Vector3(((torso_width / 2) + arm_Width/2), 0, 0), Ogre::Quaternion::IDENTITY, *mShouldernode->GetNode());
if (!mBuilder->BuildBone(*LUpperArm,Bone::BoneType::LUpperArm))
return false;
mBuilder->SetDimensions(arm_Width, arm_height, mDepth * 0.5f);
mBuilder->SetRelativePosition(Ogre::Vector3(-((torso_width / 2) + arm_Width/2), 0, 0), Ogre::Quaternion::IDENTITY, *mShouldernode->GetNode());
if (!mBuilder->BuildBone(*RUpperArm, Bone::BoneType::RUpperArm))
return false;
mBuilder->SetDimensions(arm_Width, arm_height, mDepth * 0.5f);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -(arm_height), 0), Ogre::Quaternion::IDENTITY, *LUpperArm->GetNode());
if (!mBuilder->BuildBone(*LLowerArm, Bone::BoneType::LowerArm))
return false;
mBuilder->SetDimensions(arm_Width, arm_height, mDepth * 0.5f);
mBuilder->SetRelativePosition(Ogre::Vector3(0, -(arm_height), 0), Ogre::Quaternion::IDENTITY, *RUpperArm->GetNode());
if (!mBuilder->BuildBone(*RLowerArm, Bone::BoneType::LowerArm))
return false;
mBones.push_back(LUpperArm);
mBones.push_back(RUpperArm);
mBones.push_back(LLowerArm);
mBones.push_back(RLowerArm);
if (mHasMuscle)
{
mMuscleBuilder->CreateMuscle(mShouldernode, LUpperArm, mMuscles);
mMuscleBuilder->CreateMuscle(LUpperArm, LLowerArm, mMuscles);
mMuscleBuilder->CreateMuscle(mShouldernode, RUpperArm, mMuscles);
mMuscleBuilder->CreateMuscle(RUpperArm, RLowerArm, mMuscles);
}
//add joints
btTransform localA, localB;
btVector3 JointPos;
JointPos = Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() + (torso_width / 2 + arm_Width * 0.10f));
SetJointTransform(localA, localB, JointPos, mShouldernode, LUpperArm, btVector3(0, 0, 0));
// this needed a special case
localA.getBasis().setEulerZYX(0,0,btScalar(M_PI));
localB.getBasis().setEulerZYX(0, 0, btScalar(M_PI_2));
btConeTwistConstraint* LshoulderConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *LUpperArm->GetRigidBody(), localA, localB);
LshoulderConst->setLimit(btScalar(M_PI_2), btScalar(M_PI_2), 0);
JointPos = Utils::OgreBTVector(mShouldernode->GetNode()->_getDerivedPosition() - (torso_width / 2 + arm_Width * 0.10f));
SetJointTransform(localA, localB, JointPos, mShouldernode, RUpperArm, btVector3(0, 0, 0));
localA.getBasis().setEulerZYX(0, 0, 0);
localB.getBasis().setEulerZYX(0, 0, btScalar(M_PI_2));
btConeTwistConstraint* RshoulderConst = new btConeTwistConstraint(*mShouldernode->GetRigidBody(), *RUpperArm->GetRigidBody(), localA, localB);
RshoulderConst->setLimit(btScalar(M_PI_2), btScalar(M_PI_2), 0);
JointPos = Utils::OgreBTVector(LUpperArm->GetNode()->_getDerivedPosition() - arm_height / 2);
SetJointTransform(localA, localB, JointPos, LUpperArm, LLowerArm, btVector3(0, btScalar(M_PI_2), 0));
btHingeConstraint* LArmConst = new btHingeConstraint(*LUpperArm->GetRigidBody(), *LLowerArm->GetRigidBody(), localA, localB);
LArmConst->setLimit(0, btScalar(M_PI_2));
JointPos = Utils::OgreBTVector(RUpperArm->GetNode()->_getDerivedPosition() - arm_height / 2);
SetJointTransform(localA, localB, JointPos, RUpperArm, RLowerArm, btVector3(0, btScalar(M_PI_2), 0));
btHingeConstraint* RArmConst = new btHingeConstraint(*RUpperArm->GetRigidBody(), *RLowerArm->GetRigidBody(), localA, localB);
RArmConst->setLimit(0, btScalar(M_PI_2));
mConstraints.push_back(LshoulderConst);
mConstraints.push_back(RshoulderConst);
mConstraints.push_back(LArmConst);
mConstraints.push_back(RArmConst);
//auto prestore = mMuscleBuilder->CreateMuscle(LUpperArm, LLowerArm);
//for (auto m : prestore)
// mMuscles.push_back(m);
}
// means no arms to continue
return true;
}
void SerializeDemo::initPhysics()
{
setTexturing(true);
setShadows(true);
setCameraDistance(btScalar(SCALING*50.));
setupEmptyDynamicsWorld();
#ifdef DESERIALIZE_SOFT_BODIES
m_fileLoader = new MySoftBulletWorldImporter((btSoftRigidDynamicsWorld*)m_dynamicsWorld);
#else
m_fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
#endif //DESERIALIZE_SOFT_BODIES
// fileLoader->setVerboseMode(true);
if (!m_fileLoader->loadFile("testFile.bullet"))
// if (!m_fileLoader->loadFile("../SoftDemo/testFile.bullet"))
{
///create a few basic rigid bodies and save them to testFile.bullet
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
btCollisionObject* groundObject = 0;
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
groundObject = body;
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
int numSpheres = 2;
btVector3 positions[2] = {btVector3(0.1f,0.2f,0.3f),btVector3(0.4f,0.5f,0.6f)};
btScalar radii[2] = {0.3f,0.4f};
btMultiSphereShape* colShape = new btMultiSphereShape(positions,radii,numSpheres);
//btCollisionShape* colShape = new btCapsuleShapeZ(SCALING*1,SCALING*1);
//btCollisionShape* colShape = new btCylinderShapeZ(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
float start_x = START_POS_X - ARRAY_SIZE_X/2;
float start_y = START_POS_Y;
float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
for (int k=0;k<ARRAY_SIZE_Y;k++)
{
for (int i=0;i<ARRAY_SIZE_X;i++)
{
for(int j = 0;j<ARRAY_SIZE_Z;j++)
{
startTransform.setOrigin(SCALING*btVector3(
btScalar(2.0*i + start_x),
btScalar(20+2.0*k + start_y),
btScalar(2.0*j + start_z)));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setActivationState(ISLAND_SLEEPING);
m_dynamicsWorld->addRigidBody(body);
body->setActivationState(ISLAND_SLEEPING);
}
}
}
}
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
static char* groundName = "GroundName";
serializer->registerNameForPointer(groundObject, groundName);
for (int i=0;i<m_collisionShapes.size();i++)
{
char* name = new char[20];
sprintf(name,"name%d",i);
serializer->registerNameForPointer(m_collisionShapes[i],name);
}
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*(btRigidBody*)getDynamicsWorld()->getCollisionObjectArray()[2],btVector3(0,1,0));
m_dynamicsWorld->addConstraint(p2p);
const char* name = "constraintje";
serializer->registerNameForPointer(p2p,name);
m_dynamicsWorld->serialize(serializer);
FILE* f2 = fopen("testFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
fclose(f2);
}
//clientResetScene();
}
void btGenerateInternalEdgeInfo (btBvhTriangleMeshShape*trimeshShape, btTriangleInfoMap* triangleInfoMap)
{
//the user pointer shouldn't already be used for other purposes, we intend to store connectivity info there!
if (trimeshShape->getTriangleInfoMap())
return;
trimeshShape->setTriangleInfoMap(triangleInfoMap);
btStridingMeshInterface* meshInterface = trimeshShape->getMeshInterface();
const btVector3& meshScaling = meshInterface->getScaling();
for (int partId = 0; partId< meshInterface->getNumSubParts();partId++)
{
const unsigned char *vertexbase = 0;
int numverts = 0;
PHY_ScalarType type = PHY_INTEGER;
int stride = 0;
const unsigned char *indexbase = 0;
int indexstride = 0;
int numfaces = 0;
PHY_ScalarType indicestype = PHY_INTEGER;
//PHY_ScalarType indexType=0;
btVector3 triangleVerts[3];
meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
btVector3 aabbMin,aabbMax;
for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
{
unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
for (int j=2;j>=0;j--)
{
int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
if (type == PHY_FLOAT)
{
float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
triangleVerts[j] = btVector3(
graphicsbase[0]*meshScaling.getX(),
graphicsbase[1]*meshScaling.getY(),
graphicsbase[2]*meshScaling.getZ());
}
else
{
double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
triangleVerts[j] = btVector3( btScalar(graphicsbase[0]*meshScaling.getX()), btScalar(graphicsbase[1]*meshScaling.getY()), btScalar(graphicsbase[2]*meshScaling.getZ()));
}
}
aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
aabbMin.setMin(triangleVerts[0]);
aabbMax.setMax(triangleVerts[0]);
aabbMin.setMin(triangleVerts[1]);
aabbMax.setMax(triangleVerts[1]);
aabbMin.setMin(triangleVerts[2]);
aabbMax.setMax(triangleVerts[2]);
btConnectivityProcessor connectivityProcessor;
connectivityProcessor.m_partIdA = partId;
connectivityProcessor.m_triangleIndexA = triangleIndex;
connectivityProcessor.m_triangleVerticesA = &triangleVerts[0];
connectivityProcessor.m_triangleInfoMap = triangleInfoMap;
trimeshShape->processAllTriangles(&connectivityProcessor,aabbMin,aabbMax);
}
}
}
HeightmapNode::HeightmapNode(EditorScene * _scene, LandscapeNode *_land)
: SceneNode()
, position(0,0,0)
, rotation(0)
{
SetName("HeightmapNode");
editorScene = _scene;
SetVisible(false);
land = _land;
DVASSERT(land);
//Get LandSize;
Vector3 landSize;
AABBox3 transformedBox;
land->GetBoundingBox().GetTransformedBox(land->GetWorldTransform(), transformedBox);
landSize = transformedBox.max - transformedBox.min;
heightmap = land->GetHeightmap();
sizeInMeters = landSize.x;
areaScale = sizeInMeters / heightmap->Size();
maxHeight = landSize.z;
heightScale = maxHeight / 65535.f;
float32 minHeight = 0.0f;
heightmapTexture = NULL;
debugVertices.resize(heightmap->Size() * heightmap->Size());
debugIndices.resize(heightmap->Size() * heightmap->Size() * 6);
renderDataObject = new RenderDataObject();
renderDataObject->SetStream(EVF_VERTEX, TYPE_FLOAT, 3, sizeof(LandscapeNode::LandscapeVertex), &debugVertices[0].position);
renderDataObject->SetStream(EVF_TEXCOORD0, TYPE_FLOAT, 2, sizeof(LandscapeNode::LandscapeVertex), &debugVertices[0].texCoord);
int32 step = 1;
int32 indexIndex = 0;
int32 quadWidth = heightmap->Size();
for(int32 y = 0; y < heightmap->Size() - 1; y += step)
{
for(int32 x = 0; x < heightmap->Size() - 1; x += step)
{
debugIndices[indexIndex++] = x + y * quadWidth;
debugIndices[indexIndex++] = (x + step) + y * quadWidth;
debugIndices[indexIndex++] = x + (y + step) * quadWidth;
debugIndices[indexIndex++] = (x + step) + y * quadWidth;
debugIndices[indexIndex++] = (x + step) + (y + step) * quadWidth;
debugIndices[indexIndex++] = x + (y + step) * quadWidth;
}
}
uint16 *dt = heightmap->Data();
size.x = heightmap->Size();
size.y = heightmap->Size();
hmap.resize(heightmap->Size() * heightmap->Size());
for (int32 y = 0; y < heightmap->Size(); ++y)
{
for (int32 x = 0; x < heightmap->Size(); ++x)
{
int32 index = x + (y) * heightmap->Size();
float32 mapValue = dt[index] * heightScale;
SetValueToMap(x, y, mapValue, transformedBox);
debugVertices[index].texCoord = Vector2((float32)x / (float32)(heightmap->Size() - 1),
(float32)y / (float32)(heightmap->Size() - 1));
}
}
bool flipQuadEdges = true;
colShape = new btHeightfieldTerrainShape(heightmap->Size(), heightmap->Size()
, &hmap.front(), heightScale, minHeight, maxHeight
, 2, PHY_FLOAT
, flipQuadEdges);
colShape->setLocalScaling(btVector3(areaScale, areaScale, 1.0f));
// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(0.0f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3( btScalar(position.x),
btScalar(position.y),
btScalar(maxHeight/2.0f + position.z)));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
motionSate = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,motionSate,colShape,localInertia);
rbInfo.m_friction = 0.9f;
body = new btRigidBody(rbInfo);
// btVector3 mn;
// btVector3 mx;
// body->getAabb(mn, mx);
// btVector3 sz = mx - mn;
// Logger::Debug("land size = %f, %f, %f", sz.getX(), sz.getY(), sz.getZ());
collisionObject = new btCollisionObject();
collisionObject->setWorldTransform(startTransform);
collisionObject->setCollisionShape(colShape);
editorScene->landCollisionWorld->addCollisionObject(collisionObject);
editorScene->landCollisionWorld->updateAabbs();
cursor = new LandscapeCursor();
}
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
{
//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
return;
btBvhTriangleMeshShape* trimesh = 0;
if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
else
trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
if (!triangleInfoMapPtr)
return;
int hash = btGetHash(partId0,index0);
btTriangleInfo* info = triangleInfoMapPtr->find(hash);
if (!info)
return;
btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
btVector3 v0,v1,v2;
tri_shape->getVertex(0,v0);
tri_shape->getVertex(1,v1);
tri_shape->getVertex(2,v2);
//btVector3 center = (v0+v1+v2)*btScalar(1./3.);
btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
btVector3 tri_normal;
tri_shape->calcNormal(tri_normal);
//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
btVector3 nearest;
btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);
btVector3 contact = cp.m_localPointB;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
const btTransform& tr = colObj0->getWorldTransform();
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
bool isNearEdge = false;
int numConcaveEdgeHits = 0;
int numConvexEdgeHits = 0;
btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.normalize();//is this necessary?
// Get closest edge
int bestedge=-1;
btScalar disttobestedge=BT_LARGE_FLOAT;
//
// Edge 0 -> 1
if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
btScalar len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=0;
disttobestedge=len;
}
}
// Edge 1 -> 2
if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
btScalar len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=1;
disttobestedge=len;
}
}
// Edge 2 -> 0
if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
btScalar len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=2;
disttobestedge=len;
}
}
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);
btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
#endif
if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==0 )
{
btVector3 edge(v0-v1);
isNearEdge = true;
if (info->m_edgeV0V1Angle==btScalar(0))
{
numConcaveEdgeHits++;
} else
{
bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 nA = swapFactor * tri_normal;
btQuaternion orn(edge,info->m_edgeV0V1Angle);
btVector3 computedNormalB = quatRotate(orn,tri_normal);
if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
computedNormalB*=-1;
btVector3 nB = swapFactor*computedNormalB;
btScalar NdotA = localContactNormalOnB.dot(nA);
btScalar NdotB = localContactNormalOnB.dot(nB);
bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
#ifdef DEBUG_INTERNAL_EDGE
{
btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
}
#endif //DEBUG_INTERNAL_EDGE
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
btVector3 clampedLocalNormal;
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
{
btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal. (what about cp.m_distance1?)
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
btNearestPointInLineSegment(contact,v1,v2,nearest);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
#endif
if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==1 )
{
isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 edge(v1-v2);
isNearEdge = true;
if (info->m_edgeV1V2Angle == btScalar(0))
{
numConcaveEdgeHits++;
} else
{
bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 nA = swapFactor * tri_normal;
btQuaternion orn(edge,info->m_edgeV1V2Angle);
btVector3 computedNormalB = quatRotate(orn,tri_normal);
if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
computedNormalB*=-1;
btVector3 nB = swapFactor*computedNormalB;
#ifdef DEBUG_INTERNAL_EDGE
{
btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
}
#endif //DEBUG_INTERNAL_EDGE
btScalar NdotA = localContactNormalOnB.dot(nA);
btScalar NdotB = localContactNormalOnB.dot(nB);
bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
btVector3 clampedLocalNormal;
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
{
btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
btNearestPointInLineSegment(contact,v2,v0,nearest);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
#endif
if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==2 )
{
isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 edge(v2-v0);
if (info->m_edgeV2V0Angle==btScalar(0))
{
numConcaveEdgeHits++;
} else
{
bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 nA = swapFactor * tri_normal;
btQuaternion orn(edge,info->m_edgeV2V0Angle);
btVector3 computedNormalB = quatRotate(orn,tri_normal);
if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
computedNormalB*=-1;
btVector3 nB = swapFactor*computedNormalB;
#ifdef DEBUG_INTERNAL_EDGE
{
btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
}
#endif //DEBUG_INTERNAL_EDGE
btScalar NdotA = localContactNormalOnB.dot(nA);
btScalar NdotB = localContactNormalOnB.dot(nB);
bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);
if (backFacingNormal)
{
numConcaveEdgeHits++;
}
else
{
numConvexEdgeHits++;
// printf("hitting convex edge\n");
btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
btVector3 clampedLocalNormal;
bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
if (isClamped)
{
if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
{
btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
// cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
cp.m_normalWorldOnB = newNormal;
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
}
}
}
}
}
}
#ifdef DEBUG_INTERNAL_EDGE
{
btVector3 color(0,1,1);
btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
}
#endif //DEBUG_INTERNAL_EDGE
if (isNearEdge)
{
if (numConcaveEdgeHits>0)
{
if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
{
//fix tri_normal so it pointing the same direction as the current local contact normal
if (tri_normal.dot(localContactNormalOnB) < 0)
{
tri_normal *= -1;
}
cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal;
} else
{
btVector3 newNormal = tri_normal *frontFacing;
//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
btScalar d = newNormal.dot(localContactNormalOnB) ;
if (d< 0)
{
return;
}
//modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal;
}
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
}
}
}
void BasicDemo::initPhysics()
{
setTexturing(true);
setShadows(true);
setCameraDistance(btScalar(SCALING*50.));
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
{
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
groundShape->calculateLocalInertia(mass,localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
float start_x = START_POS_X - ARRAY_SIZE_X/2;
float start_y = START_POS_Y;
float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
for (int k=0; k<ARRAY_SIZE_Y; k++)
{
for (int i=0; i<ARRAY_SIZE_X; i++)
{
for(int j = 0; j<ARRAY_SIZE_Z; j++)
{
startTransform.setOrigin(SCALING*btVector3(
2.0*i + start_x,
20+2.0*k + start_y,
2.0*j + start_z));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//body->setContactProcessingThreshold(colShape->getContactBreakingThreshold());
body->setActivationState(ISLAND_SLEEPING);
m_dynamicsWorld->addRigidBody(body);
body->setActivationState(ISLAND_SLEEPING);
}
}
}
}
clientResetScene();
}
bool btSubsimplexConvexCast::calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
m_simplexSolver->reset();
btVector3 linVelA,linVelB;
linVelA = toA.getOrigin()-fromA.getOrigin();
linVelB = toB.getOrigin()-fromB.getOrigin();
btScalar lambda = btScalar(0.);
btTransform interpolatedTransA = fromA;
btTransform interpolatedTransB = fromB;
///take relative motion
btVector3 r = (linVelA-linVelB);
btVector3 v;
btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r*fromA.getBasis()));
btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r*fromB.getBasis()));
v = supVertexA-supVertexB;
int maxIter = sscc_MAX_ITERATIONS;
btVector3 n;
n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
bool hasResult = false;
btVector3 c;
btScalar lastLambda = lambda;
btScalar dist2 = v.length2();
#ifdef BT_USE_DOUBLE_PRECISION
btScalar epsilon = btScalar(0.0001);
#else
btScalar epsilon = btScalar(0.0001);
#endif //BT_USE_DOUBLE_PRECISION
btVector3 w,p;
btScalar VdotR;
while ( (dist2 > epsilon) && maxIter--)
{
supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v*interpolatedTransA.getBasis()));
supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v*interpolatedTransB.getBasis()));
w = supVertexA-supVertexB;
btScalar VdotW = v.dot(w);
if (lambda > btScalar(1.0))
{
return false;
}
if ( VdotW > btScalar(0.))
{
VdotR = v.dot(r);
if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
return false;
else
{
lambda = lambda - VdotW / VdotR;
//interpolate to next lambda
// x = s + lambda * r;
interpolatedTransA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);
interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);
//m_simplexSolver->reset();
//check next line
w = supVertexA-supVertexB;
lastLambda = lambda;
n = v;
hasResult = true;
}
}
///Just like regular GJK only add the vertex if it isn't already (close) to current vertex, it would lead to divisions by zero and NaN etc.
if (!m_simplexSolver->inSimplex(w))
m_simplexSolver->addVertex( w, supVertexA , supVertexB);
if (m_simplexSolver->closest(v))
{
dist2 = v.length2();
hasResult = true;
//todo: check this normal for validity
//n=v;
//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
//printf("DIST2=%f\n",dist2);
//printf("numverts = %i\n",m_simplexSolver->numVertices());
} else
{
dist2 = btScalar(0.);
}
}
//int numiter = sscc_MAX_ITERATIONS - maxIter;
// printf("number of iterations: %d", numiter);
//don't report a time of impact when moving 'away' from the hitnormal
result.m_fraction = lambda;
if (n.length2() >= (SIMD_EPSILON*SIMD_EPSILON))
result.m_normal = n.normalized();
else
result.m_normal = btVector3(btScalar(0.0), btScalar(0.0), btScalar(0.0));
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if (result.m_normal.dot(r)>=-result.m_allowedPenetration)
return false;
btVector3 hitA,hitB;
m_simplexSolver->compute_points(hitA,hitB);
result.m_hitPoint=hitB;
return true;
}
void myinit()
{
// GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) };
GLfloat light_ambient[] = { btScalar(1.0), btScalar(1.2), btScalar(0.2), btScalar(1.0) };
GLfloat light_diffuse[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0) };
GLfloat light_specular[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0 )};
/* light_position is NOT default value */
GLfloat light_position0[] = { btScalar(1000.0), btScalar(1000.0), btScalar(1000.0), btScalar(0.0 )};
GLfloat light_position1[] = { btScalar(-1.0), btScalar(-10.0), btScalar(-1.0), btScalar(0.0) };
glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient);
glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHT1);
// glShadeModel(GL_FLAT);//GL_SMOOTH);
glShadeModel(GL_SMOOTH);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
glClearColor(float(0.7),float(0.7),float(0.7),float(0));
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
static bool m_textureenabled = true;
static bool m_textureinitialized = false;
if(m_textureenabled)
{
if(!m_textureinitialized)
{
glActiveTexture(GL_TEXTURE0);
GLubyte* image=new GLubyte[256*256*3];
for(int y=0;y<256;++y)
{
const int t=y>>5;
GLubyte* pi=image+y*256*3;
for(int x=0;x<256;++x)
{
const int s=x>>5;
const GLubyte b=180;
GLubyte c=b+((s+t&1)&1)*(255-b);
pi[0]=255;
pi[1]=c;
pi[2]=c;
pi+=3;
}
}
glGenTextures(1,(GLuint*)&m_texturehandle);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
glTexEnvf(GL_TEXTURE_ENV,GL_TEXTURE_ENV_MODE,GL_MODULATE);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
gluBuild2DMipmaps(GL_TEXTURE_2D,3,256,256,GL_RGB,GL_UNSIGNED_BYTE,image);
delete[] image;
m_textureinitialized=true;
}
// glMatrixMode(GL_TEXTURE);
// glLoadIdentity();
// glMatrixMode(GL_MODELVIEW);
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
} else
bool m_useWalkDirection;
btScalar m_velocityTimeInterval;
int m_upAxis;
static btVector3* getUpAxisDirections();
bool m_interpolateUp;
bool full_drop;
bool bounce_fix;
btVector3 computeReflectionDirection(const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent(const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent(const btVector3& direction, const btVector3& normal);
bool recoverFromPenetration(btCollisionWorld* collisionWorld);
void stepUp(btCollisionWorld* collisionWorld);
void updateTargetPositionBasedOnCollision(const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe(btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown(btCollisionWorld* collisionWorld, btScalar dt);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
KinematicCharacterController(btPairCachingGhostObject* ghostObject, btConvexShape* convexShape, btScalar stepHeight, int upAxis = 1);
~KinematicCharacterController();
///btActionInterface interface
virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime)
{
preStep(collisionWorld);
playerStep(collisionWorld, deltaTime);
bool btBulletWorldImporter::loadFileFromMemory( bParse::btBulletFile* bulletFile2)
{
bool ok = (bulletFile2->getFlags()& bParse::FD_OK)!=0;
if (ok)
bulletFile2->parse(m_verboseDumpAllTypes);
else
return false;
if (m_verboseDumpAllTypes)
{
bulletFile2->dumpChunks(bulletFile2->getFileDNA());
}
int i;
btHashMap<btHashPtr,btCollisionShape*> shapeMap;
for (i=0;i<bulletFile2->m_collisionShapes.size();i++)
{
btCollisionShapeData* shapeData = (btCollisionShapeData*)bulletFile2->m_collisionShapes[i];
btCollisionShape* shape = convertCollisionShape(shapeData);
if (shape)
shapeMap.insert(shapeData,shape);
}
btHashMap<btHashPtr,btCollisionObject*> bodyMap;
for (i=0;i<bulletFile2->m_rigidBodies.size();i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btRigidBodyDoubleData* colObjData = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i];
btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f);
btVector3 localInertia;
localInertia.setZero();
btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape);
if (shapePtr && *shapePtr)
{
btTransform startTransform;
startTransform.deSerializeDouble(colObjData->m_collisionObjectData.m_worldTransform);
// startTransform.setBasis(btMatrix3x3::getIdentity());
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
}
bool isDynamic = mass!=0.f;
btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape);
bodyMap.insert(colObjData,body);
} else
{
printf("error: no shape found\n");
}
} else
{
btRigidBodyFloatData* colObjData = (btRigidBodyFloatData*)bulletFile2->m_rigidBodies[i];
btScalar mass = btScalar(colObjData->m_inverseMass? 1.f/colObjData->m_inverseMass : 0.f);
btVector3 localInertia;
localInertia.setZero();
btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionObjectData.m_collisionShape);
if (shapePtr && *shapePtr)
{
btTransform startTransform;
startTransform.deSerializeFloat(colObjData->m_collisionObjectData.m_worldTransform);
// startTransform.setBasis(btMatrix3x3::getIdentity());
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
}
bool isDynamic = mass!=0.f;
btRigidBody* body = createRigidBody(isDynamic,mass,startTransform,shape);
bodyMap.insert(colObjData,body);
} else
{
printf("error: no shape found\n");
}
}
}
for (i=0;i<bulletFile2->m_collisionObjects.size();i++)
{
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btCollisionObjectDoubleData* colObjData = (btCollisionObjectDoubleData*)bulletFile2->m_collisionObjects[i];
btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionShape);
if (shapePtr && *shapePtr)
{
btTransform startTransform;
startTransform.deSerializeDouble(colObjData->m_worldTransform);
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
btCollisionObject* body = createCollisionObject(startTransform,shape);
bodyMap.insert(colObjData,body);
} else
{
printf("error: no shape found\n");
}
} else
{
btCollisionObjectFloatData* colObjData = (btCollisionObjectFloatData*)bulletFile2->m_collisionObjects[i];
btCollisionShape** shapePtr = shapeMap.find(colObjData->m_collisionShape);
if (shapePtr && *shapePtr)
{
btTransform startTransform;
startTransform.deSerializeFloat(colObjData->m_worldTransform);
btCollisionShape* shape = (btCollisionShape*)*shapePtr;
btCollisionObject* body = createCollisionObject(startTransform,shape);
bodyMap.insert(colObjData,body);
} else
{
printf("error: no shape found\n");
}
}
printf("bla");
}
for (i=0;i<bulletFile2->m_constraints.size();i++)
{
btTypedConstraintData* constraintData = (btTypedConstraintData*)bulletFile2->m_constraints[i];
btCollisionObject** colAptr = bodyMap.find(constraintData->m_rbA);
btCollisionObject** colBptr = bodyMap.find(constraintData->m_rbB);
btRigidBody* rbA = 0;
btRigidBody* rbB = 0;
if (colAptr)
{
rbA = btRigidBody::upcast(*colAptr);
if (!rbA)
rbA = &getFixedBody();
}
if (colBptr)
{
rbB = btRigidBody::upcast(*colBptr);
if (!rbB)
rbB = &getFixedBody();
}
switch (constraintData->m_objectType)
{
case POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint* constraint = 0;
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btPoint2PointConstraintDoubleData* p2pData = (btPoint2PointConstraintDoubleData*)constraintData;
if (rbA && rbB)
{
btVector3 pivotInA,pivotInB;
pivotInA.deSerializeDouble(p2pData->m_pivotInA);
pivotInB.deSerializeDouble(p2pData->m_pivotInB);
constraint = new btPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB);
} else
{
btVector3 pivotInA;
pivotInA.deSerializeDouble(p2pData->m_pivotInA);
constraint = new btPoint2PointConstraint(*rbA,pivotInA);
}
} else
{
btPoint2PointConstraintFloatData* p2pData = (btPoint2PointConstraintFloatData*)constraintData;
if (rbA&& rbB)
{
btVector3 pivotInA,pivotInB;
pivotInA.deSerializeFloat(p2pData->m_pivotInA);
pivotInB.deSerializeFloat(p2pData->m_pivotInB);
constraint = new btPoint2PointConstraint(*rbA,*rbB,pivotInA,pivotInB);
} else
{
btVector3 pivotInA;
pivotInA.deSerializeFloat(p2pData->m_pivotInA);
constraint = new btPoint2PointConstraint(*rbA,pivotInA);
}
}
m_dynamicsWorld->addConstraint(constraint,constraintData->m_disableCollisionsBetweenLinkedBodies!=0);
constraint->setDbgDrawSize(constraintData->m_dbgDrawSize);
break;
}
case HINGE_CONSTRAINT_TYPE:
{
btHingeConstraint* hinge = 0;
if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
{
btHingeConstraintDoubleData* hingeData = (btHingeConstraintDoubleData*)constraintData;
if (rbA&& rbB)
{
btTransform rbAFrame,rbBFrame;
rbAFrame.deSerializeDouble(hingeData->m_rbAFrame);
rbBFrame.deSerializeDouble(hingeData->m_rbBFrame);
hinge = new btHingeConstraint(*rbA,*rbB,rbAFrame,rbBFrame,hingeData->m_useReferenceFrameA!=0);
} else
{
btTransform rbAFrame;
rbAFrame.deSerializeDouble(hingeData->m_rbAFrame);
hinge = new btHingeConstraint(*rbA,rbAFrame,hingeData->m_useReferenceFrameA!=0);
}
if (hingeData->m_enableAngularMotor)
{
hinge->enableAngularMotor(true,hingeData->m_motorTargetVelocity,hingeData->m_maxMotorImpulse);
}
hinge->setAngularOnly(hingeData->m_angularOnly!=0);
hinge->setLimit(btScalar(hingeData->m_lowerLimit),btScalar(hingeData->m_upperLimit),btScalar(hingeData->m_limitSoftness),btScalar(hingeData->m_biasFactor),btScalar(hingeData->m_relaxationFactor));
} else
{
btHingeConstraintFloatData* hingeData = (btHingeConstraintFloatData*)constraintData;
if (rbA&& rbB)
{
btTransform rbAFrame,rbBFrame;
rbAFrame.deSerializeFloat(hingeData->m_rbAFrame);
rbBFrame.deSerializeFloat(hingeData->m_rbBFrame);
hinge = new btHingeConstraint(*rbA,*rbB,rbAFrame,rbBFrame,hingeData->m_useReferenceFrameA!=0);
} else
{
btTransform rbAFrame;
rbAFrame.deSerializeFloat(hingeData->m_rbAFrame);
hinge = new btHingeConstraint(*rbA,rbAFrame,hingeData->m_useReferenceFrameA!=0);
}
if (hingeData->m_enableAngularMotor)
{
hinge->enableAngularMotor(true,hingeData->m_motorTargetVelocity,hingeData->m_maxMotorImpulse);
}
hinge->setAngularOnly(hingeData->m_angularOnly!=0);
hinge->setLimit(btScalar(hingeData->m_lowerLimit),btScalar(hingeData->m_upperLimit),btScalar(hingeData->m_limitSoftness),btScalar(hingeData->m_biasFactor),btScalar(hingeData->m_relaxationFactor));
}
m_dynamicsWorld->addConstraint(hinge,constraintData->m_disableCollisionsBetweenLinkedBodies!=0);
hinge->setDbgDrawSize(constraintData->m_dbgDrawSize);
break;
}
case CONETWIST_CONSTRAINT_TYPE:
{
btConeTwistConstraintData* coneData = (btConeTwistConstraintData*)constraintData;
btConeTwistConstraint* coneTwist = 0;
if (rbA&& rbB)
{
btTransform rbAFrame,rbBFrame;
rbAFrame.deSerializeFloat(coneData->m_rbAFrame);
rbBFrame.deSerializeFloat(coneData->m_rbBFrame);
coneTwist = new btConeTwistConstraint(*rbA,*rbB,rbAFrame,rbBFrame);
} else
{
btTransform rbAFrame;
rbAFrame.deSerializeFloat(coneData->m_rbAFrame);
coneTwist = new btConeTwistConstraint(*rbA,rbAFrame);
}
coneTwist->setLimit(coneData->m_swingSpan1,coneData->m_swingSpan2,coneData->m_twistSpan,coneData->m_limitSoftness,coneData->m_biasFactor,coneData->m_relaxationFactor);
coneTwist->setDamping(coneData->m_damping);
m_dynamicsWorld->addConstraint(coneTwist,constraintData->m_disableCollisionsBetweenLinkedBodies!=0);
coneTwist->setDbgDrawSize(constraintData->m_dbgDrawSize);
break;
}
case D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraintData* dofData = (btGeneric6DofConstraintData*)constraintData;
btGeneric6DofConstraint* dof = 0;
if (rbA&& rbB)
{
btTransform rbAFrame,rbBFrame;
rbAFrame.deSerializeFloat(dofData->m_rbAFrame);
rbBFrame.deSerializeFloat(dofData->m_rbBFrame);
dof = new btGeneric6DofConstraint(*rbA,*rbB,rbAFrame,rbBFrame,dofData->m_useLinearReferenceFrameA!=0);
} else
{
btTransform rbBFrame;
rbBFrame.deSerializeFloat(dofData->m_rbBFrame);
dof = new btGeneric6DofConstraint(*rbB,rbBFrame,dofData->m_useLinearReferenceFrameA!=0);
}
btVector3 angLowerLimit,angUpperLimit, linLowerLimit,linUpperlimit;
angLowerLimit.deSerializeFloat(dofData->m_angularLowerLimit);
angUpperLimit.deSerializeFloat(dofData->m_angularUpperLimit);
linLowerLimit.deSerializeFloat(dofData->m_linearLowerLimit);
linUpperlimit.deSerializeFloat(dofData->m_linearUpperLimit);
dof->setAngularLowerLimit(angLowerLimit);
dof->setAngularUpperLimit(angUpperLimit);
dof->setLinearLowerLimit(linLowerLimit);
dof->setLinearUpperLimit(linUpperlimit);
m_dynamicsWorld->addConstraint(dof,constraintData->m_disableCollisionsBetweenLinkedBodies!=0);
dof->setDbgDrawSize(constraintData->m_dbgDrawSize);
break;
}
case SLIDER_CONSTRAINT_TYPE:
{
btSliderConstraintData* sliderData = (btSliderConstraintData*)constraintData;
btSliderConstraint* slider = 0;
if (rbA&& rbB)
{
btTransform rbAFrame,rbBFrame;
rbAFrame.deSerializeFloat(sliderData->m_rbAFrame);
rbBFrame.deSerializeFloat(sliderData->m_rbBFrame);
slider = new btSliderConstraint(*rbA,*rbB,rbAFrame,rbBFrame,sliderData->m_useLinearReferenceFrameA!=0);
} else
{
btTransform rbBFrame;
rbBFrame.deSerializeFloat(sliderData->m_rbBFrame);
slider = new btSliderConstraint(*rbB,rbBFrame,sliderData->m_useLinearReferenceFrameA!=0);
}
slider->setLowerLinLimit(sliderData->m_linearLowerLimit);
slider->setUpperLinLimit(sliderData->m_linearUpperLimit);
slider->setLowerAngLimit(sliderData->m_angularLowerLimit);
slider->setUpperAngLimit(sliderData->m_angularUpperLimit);
slider->setUseFrameOffset(sliderData->m_useOffsetForConstraintFrame!=0);
m_dynamicsWorld->addConstraint(slider,constraintData->m_disableCollisionsBetweenLinkedBodies!=0);
slider->setDbgDrawSize(constraintData->m_dbgDrawSize);
break;
}
default:
{
printf("unknown constraint type\n");
}
};
}
return true;
}
void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax)
{
m_useQuantization = useQuantizedAabbCompression;
// NodeArray triangleNodes;
struct NodeTriangleCallback : public btInternalTriangleIndexCallback
{
NodeArray& m_triangleNodes;
NodeTriangleCallback& operator=(NodeTriangleCallback& other)
{
m_triangleNodes = other.m_triangleNodes;
return *this;
}
NodeTriangleCallback(NodeArray& triangleNodes)
:m_triangleNodes(triangleNodes)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
btOptimizedBvhNode node;
btVector3 aabbMin,aabbMax;
aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
aabbMin.setMin(triangle[0]);
aabbMax.setMax(triangle[0]);
aabbMin.setMin(triangle[1]);
aabbMax.setMax(triangle[1]);
aabbMin.setMin(triangle[2]);
aabbMax.setMax(triangle[2]);
//with quantization?
node.m_aabbMinOrg = aabbMin;
node.m_aabbMaxOrg = aabbMax;
node.m_escapeIndex = -1;
//for child nodes
node.m_subPart = partId;
node.m_triangleIndex = triangleIndex;
m_triangleNodes.push_back(node);
}
};
struct QuantizedNodeTriangleCallback : public btInternalTriangleIndexCallback
{
QuantizedNodeArray& m_triangleNodes;
const btQuantizedBvh* m_optimizedTree; // for quantization
QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other)
{
m_triangleNodes = other.m_triangleNodes;
m_optimizedTree = other.m_optimizedTree;
return *this;
}
QuantizedNodeTriangleCallback(QuantizedNodeArray& triangleNodes,const btQuantizedBvh* tree)
:m_triangleNodes(triangleNodes),m_optimizedTree(tree)
{
}
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{
// The partId and triangle index must fit in the same (positive) integer
btAssert(partId < (1<<MAX_NUM_PARTS_IN_BITS));
btAssert(triangleIndex < (1<<(31-MAX_NUM_PARTS_IN_BITS)));
//negative indices are reserved for escapeIndex
btAssert(triangleIndex>=0);
btQuantizedBvhNode node;
btVector3 aabbMin,aabbMax;
aabbMin.setValue(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
aabbMax.setValue(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
aabbMin.setMin(triangle[0]);
aabbMax.setMax(triangle[0]);
aabbMin.setMin(triangle[1]);
aabbMax.setMax(triangle[1]);
aabbMin.setMin(triangle[2]);
aabbMax.setMax(triangle[2]);
//PCK: add these checks for zero dimensions of aabb
const btScalar MIN_AABB_DIMENSION = btScalar(0.002);
const btScalar MIN_AABB_HALF_DIMENSION = btScalar(0.001);
if (aabbMax.x() - aabbMin.x() < MIN_AABB_DIMENSION)
{
aabbMax.setX(aabbMax.x() + MIN_AABB_HALF_DIMENSION);
aabbMin.setX(aabbMin.x() - MIN_AABB_HALF_DIMENSION);
}
if (aabbMax.y() - aabbMin.y() < MIN_AABB_DIMENSION)
{
aabbMax.setY(aabbMax.y() + MIN_AABB_HALF_DIMENSION);
aabbMin.setY(aabbMin.y() - MIN_AABB_HALF_DIMENSION);
}
if (aabbMax.z() - aabbMin.z() < MIN_AABB_DIMENSION)
{
aabbMax.setZ(aabbMax.z() + MIN_AABB_HALF_DIMENSION);
aabbMin.setZ(aabbMin.z() - MIN_AABB_HALF_DIMENSION);
}
m_optimizedTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
m_optimizedTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
m_triangleNodes.push_back(node);
}
};
int numLeafNodes = 0;
if (m_useQuantization)
{
//initialize quantization values
setQuantizationValues(bvhAabbMin,bvhAabbMax);
QuantizedNodeTriangleCallback callback(m_quantizedLeafNodes,this);
triangles->InternalProcessAllTriangles(&callback,m_bvhAabbMin,m_bvhAabbMax);
//now we have an array of leafnodes in m_leafNodes
numLeafNodes = m_quantizedLeafNodes.size();
m_quantizedContiguousNodes.resize(2*numLeafNodes);
} else
{
NodeTriangleCallback callback(m_leafNodes);
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
//now we have an array of leafnodes in m_leafNodes
numLeafNodes = m_leafNodes.size();
m_contiguousNodes.resize(2*numLeafNodes);
}
m_curNodeIndex = 0;
buildTree(0,numLeafNodes);
///if the entire tree is small then subtree size, we need to create a header info for the tree
if(m_useQuantization && !m_SubtreeHeaders.size())
{
btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand();
subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]);
subtree.m_rootNodeIndex = 0;
subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex();
}
//PCK: update the copy of the size
m_subtreeHeaderCount = m_SubtreeHeaders.size();
//PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
m_quantizedLeafNodes.clear();
m_leafNodes.clear();
}
btRigidBody& btBulletWorldImporter::getFixedBody()
{
static btRigidBody s_fixed(0, 0,0);
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
return s_fixed;
}
btScalar btTranslationalLimitMotor::solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
btRigidBody& body2,btSolverBody& bodyB,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;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2;
bodyB.getVelocityInLocalPointObsolete(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);
bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
return normalImpulse;
}
btCollisionShape* btBulletWorldImporter::convertCollisionShape( btCollisionShapeData* shapeData )
{
btCollisionShape* shape = 0;
switch (shapeData->m_shapeType)
{
case STATIC_PLANE_PROXYTYPE:
{
btStaticPlaneShapeData* planeData = (btStaticPlaneShapeData*)shapeData;
btVector3 planeNormal,localScaling;
planeNormal.deSerializeFloat(planeData->m_planeNormal);
localScaling.deSerializeFloat(planeData->m_localScaling);
shape = new btStaticPlaneShape(planeNormal,btScalar(planeData->m_planeConstant));
shape->setLocalScaling(localScaling);
break;
}
case GIMPACT_SHAPE_PROXYTYPE:
{
btGImpactMeshShapeData* gimpactData = (btGImpactMeshShapeData*) shapeData;
if (gimpactData->m_gimpactSubType == CONST_GIMPACT_TRIMESH_SHAPE)
{
btTriangleIndexVertexArray* meshInterface = createMeshInterface(gimpactData->m_meshInterface);
btGImpactMeshShape* gimpactShape = createGimpactShape(meshInterface);
btVector3 localScaling;
localScaling.deSerializeFloat(gimpactData->m_localScaling);
gimpactShape->setLocalScaling(localScaling);
gimpactShape->setMargin(btScalar(gimpactData->m_collisionMargin));
gimpactShape->updateBound();
shape = gimpactShape;
} else
{
printf("unsupported gimpact sub type\n");
}
break;
}
case CYLINDER_SHAPE_PROXYTYPE:
case CAPSULE_SHAPE_PROXYTYPE:
case BOX_SHAPE_PROXYTYPE:
case SPHERE_SHAPE_PROXYTYPE:
case MULTI_SPHERE_SHAPE_PROXYTYPE:
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
btConvexInternalShapeData* bsd = (btConvexInternalShapeData*)shapeData;
btVector3 implicitShapeDimensions;
implicitShapeDimensions.deSerializeFloat(bsd->m_implicitShapeDimensions);
btVector3 localScaling;
localScaling.deSerializeFloat(bsd->m_localScaling);
btVector3 margin(bsd->m_collisionMargin,bsd->m_collisionMargin,bsd->m_collisionMargin);
switch (shapeData->m_shapeType)
{
case BOX_SHAPE_PROXYTYPE:
{
shape = createBoxShape(implicitShapeDimensions/localScaling+margin);
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
shape = createSphereShape(implicitShapeDimensions.getX());
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
btCapsuleShapeData* capData = (btCapsuleShapeData*)shapeData;
switch (capData->m_upAxis)
{
case 0:
{
shape = createCapsuleShapeX(implicitShapeDimensions.getY(),2*implicitShapeDimensions.getX());
break;
}
case 1:
{
shape = createCapsuleShapeY(implicitShapeDimensions.getX(),2*implicitShapeDimensions.getY());
break;
}
case 2:
{
shape = createCapsuleShapeZ(implicitShapeDimensions.getX(),2*implicitShapeDimensions.getZ());
break;
}
default:
{
printf("error: wrong up axis for btCapsuleShape\n");
}
};
break;
}
case CYLINDER_SHAPE_PROXYTYPE:
{
btCylinderShapeData* cylData = (btCylinderShapeData*) shapeData;
btVector3 halfExtents = implicitShapeDimensions+margin;
switch (cylData->m_upAxis)
{
case 0:
{
shape = createCylinderShapeX(halfExtents.getY(),halfExtents.getX());
break;
}
case 1:
{
shape = createCylinderShapeY(halfExtents.getX(),halfExtents.getY());
break;
}
case 2:
{
shape = createCylinderShapeZ(halfExtents.getX(),halfExtents.getZ());
break;
}
default:
{
printf("unknown Cylinder up axis\n");
}
};
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
btMultiSphereShapeData* mss = (btMultiSphereShapeData*)bsd;
int numSpheres = mss->m_localPositionArraySize;
btAlignedObjectArray<btVector3> tmpPos;
btAlignedObjectArray<btScalar> radii;
radii.resize(numSpheres);
tmpPos.resize(numSpheres);
int i;
for ( i=0;i<numSpheres;i++)
{
tmpPos[i].deSerializeFloat(mss->m_localPositionArrayPtr[i].m_pos);
radii[i] = mss->m_localPositionArrayPtr[i].m_radius;
}
shape = new btMultiSphereShape(&tmpPos[0],&radii[0],numSpheres);
break;
}
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
// int sz = sizeof(btConvexHullShapeData);
// int sz2 = sizeof(btConvexInternalShapeData);
// int sz3 = sizeof(btCollisionShapeData);
btConvexHullShapeData* convexData = (btConvexHullShapeData*)bsd;
int numPoints = convexData->m_numUnscaledPoints;
btAlignedObjectArray<btVector3> tmpPoints;
tmpPoints.resize(numPoints);
int i;
for ( i=0;i<numPoints;i++)
{
#ifdef BT_USE_DOUBLE_PRECISION
if (convexData->m_unscaledPointsDoublePtr)
tmpPoints[i].deSerialize(convexData->m_unscaledPointsDoublePtr[i]);
if (convexData->m_unscaledPointsFloatPtr)
tmpPoints[i].deSerializeFloat(convexData->m_unscaledPointsFloatPtr[i]);
#else
if (convexData->m_unscaledPointsFloatPtr)
tmpPoints[i].deSerialize(convexData->m_unscaledPointsFloatPtr[i]);
if (convexData->m_unscaledPointsDoublePtr)
tmpPoints[i].deSerializeDouble(convexData->m_unscaledPointsDoublePtr[i]);
#endif //BT_USE_DOUBLE_PRECISION
}
btConvexHullShape* hullShape = createConvexHullShape();
for (i=0;i<numPoints;i++)
{
hullShape->addPoint(tmpPoints[i]);
}
shape = hullShape;
break;
}
default:
{
printf("error: cannot create shape type (%d)\n",shapeData->m_shapeType);
}
}
if (shape)
{
shape->setMargin(bsd->m_collisionMargin);
btVector3 localScaling;
localScaling.deSerializeFloat(bsd->m_localScaling);
shape->setLocalScaling(localScaling);
}
break;
}
case TRIANGLE_MESH_SHAPE_PROXYTYPE:
{
btTriangleMeshShapeData* trimesh = (btTriangleMeshShapeData*)shapeData;
btTriangleIndexVertexArray* meshInterface = createMeshInterface(trimesh->m_meshInterface);
btVector3 scaling; scaling.deSerializeFloat(trimesh->m_meshInterface.m_scaling);
meshInterface->setScaling(scaling);
btCollisionShape* trimeshShape = createBvhTriangleMeshShape(meshInterface);
trimeshShape->setMargin(trimesh->m_collisionMargin);
shape = trimeshShape;
//printf("trimesh->m_collisionMargin=%f\n",trimesh->m_collisionMargin);
break;
}
case COMPOUND_SHAPE_PROXYTYPE:
{
btCompoundShapeData* compoundData = (btCompoundShapeData*)shapeData;
btCompoundShape* compoundShape = createCompoundShape();
btAlignedObjectArray<btCollisionShape*> childShapes;
for (int i=0;i<compoundData->m_numChildShapes;i++)
{
btCollisionShape* childShape = convertCollisionShape(compoundData->m_childShapePtr[i].m_childShape);
if (childShape)
{
btTransform localTransform;
localTransform.deSerializeFloat(compoundData->m_childShapePtr[i].m_transform);
compoundShape->addChildShape(localTransform,childShape);
} else
{
printf("error: couldn't create childShape for compoundShape\n");
}
}
shape = compoundShape;
break;
}
default:
{
printf("unsupported shape type (%d)\n",shapeData->m_shapeType);
}
}
return shape;
}
static btDbvtNode* topdown(btDbvt* pdbvt,
tNodeArray& leaves,
int bu_treshold)
{
static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0),
btVector3(0,0,1)};
if(leaves.size()>1)
{
if(leaves.size()>bu_treshold)
{
const btDbvtVolume vol=bounds(leaves);
const btVector3 org=vol.Center();
tNodeArray sets[2];
int bestaxis=-1;
int bestmidp=leaves.size();
int splitcount[3][2]={{0,0},{0,0},{0,0}};
int i;
for( i=0;i<leaves.size();++i)
{
const btVector3 x=leaves[i]->volume.Center()-org;
for(int j=0;j<3;++j)
{
++splitcount[j][btDot(x,axis[j])>0?1:0];
}
}
for( i=0;i<3;++i)
{
if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
{
const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
if(midp<bestmidp)
{
bestaxis=i;
bestmidp=midp;
}
}
}
if(bestaxis>=0)
{
sets[0].reserve(splitcount[bestaxis][0]);
sets[1].reserve(splitcount[bestaxis][1]);
split(leaves,sets[0],sets[1],org,axis[bestaxis]);
}
else
{
sets[0].reserve(leaves.size()/2+1);
sets[1].reserve(leaves.size()/2);
for(int i=0,ni=leaves.size();i<ni;++i)
{
sets[i&1].push_back(leaves[i]);
}
}
btDbvtNode* node=createnode(pdbvt,0,vol,0);
node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
node->childs[0]->parent=node;
node->childs[1]->parent=node;
return(node);
}
else
{
bottomup(pdbvt,leaves);
return(leaves[0]);
}
}
return(leaves[0]);
}
void RollingFrictionDemo::initPhysics()
{
m_guiHelper->setUpAxis(2);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
m_broadphase = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
// m_dynamicsWorld->getSolverInfo().m_singleAxisRollingFrictionThreshold = 0.f;//faster but lower quality
m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
{
///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(25.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, 0, -28));
groundTransform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI * 0.03));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(.5);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
{
///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.), btScalar(100.), btScalar(50.)));
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, 0, -54));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(.1);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
#define NUM_SHAPES 10
btCollisionShape* colShapes[NUM_SHAPES] = {
new btSphereShape(btScalar(0.5)),
new btCapsuleShape(0.25, 0.5),
new btCapsuleShapeX(0.25, 0.5),
new btCapsuleShapeZ(0.25, 0.5),
new btConeShape(0.25, 0.5),
new btConeShapeX(0.25, 0.5),
new btConeShapeZ(0.25, 0.5),
new btCylinderShape(btVector3(0.25, 0.5, 0.25)),
new btCylinderShapeX(btVector3(0.5, 0.25, 0.25)),
new btCylinderShapeZ(btVector3(0.25, 0.25, 0.5)),
};
for (int i = 0; i < NUM_SHAPES; i++)
m_collisionShapes.push_back(colShapes[i]);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
float start_x = START_POS_X - ARRAY_SIZE_X / 2;
float start_y = START_POS_Y;
float start_z = START_POS_Z - ARRAY_SIZE_Z / 2;
{
int shapeIndex = 0;
for (int k = 0; k < ARRAY_SIZE_Y; k++)
{
for (int i = 0; i < ARRAY_SIZE_X; i++)
{
for (int j = 0; j < ARRAY_SIZE_Z; j++)
{
startTransform.setOrigin(SCALING * btVector3(
btScalar(2.0 * i + start_x),
btScalar(2.0 * j + start_z),
btScalar(20 + 2.0 * k + start_y)));
shapeIndex++;
btCollisionShape* colShape = colShapes[shapeIndex % NUM_SHAPES];
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
colShape->calculateLocalInertia(mass, localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setFriction(1.f);
body->setRollingFriction(.1);
body->setSpinningFriction(0.1);
body->setAnisotropicFriction(colShape->getAnisotropicRollingFrictionDirection(), btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
m_dynamicsWorld->addRigidBody(body);
}
}
}
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
if (0)
{
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
char resourcePath[1024];
if (b3ResourcePath::findResourcePath("slope.bullet", resourcePath, 1024))
{
FILE* f = fopen(resourcePath, "wb");
fwrite(s->getBufferPointer(), s->getCurrentBufferSize(), 1, f);
fclose(f);
}
}
}
void btHeightfieldTerrainShape::initialize
(
int heightStickWidth, int heightStickLength, const void* heightfieldData,
btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
PHY_ScalarType hdt, bool flipQuadEdges
)
{
// validation
btAssert(heightStickWidth > 1 && "bad width");
btAssert(heightStickLength > 1 && "bad length");
btAssert(heightfieldData && "null heightfield data");
// btAssert(heightScale) -- do we care? Trust caller here
btAssert(minHeight <= maxHeight && "bad min/max height");
btAssert(upAxis >= 0 && upAxis < 3 &&
"bad upAxis--should be in range [0,2]");
btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
"Bad height data type enum");
// initialize member variables
m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
m_heightStickWidth = heightStickWidth;
m_heightStickLength = heightStickLength;
m_minHeight = minHeight;
m_maxHeight = maxHeight;
m_width = (btScalar) (heightStickWidth - 1);
m_length = (btScalar) (heightStickLength - 1);
m_heightScale = heightScale;
m_heightfieldDataUnknown = heightfieldData;
m_heightDataType = hdt;
m_flipQuadEdges = flipQuadEdges;
m_useDiamondSubdivision = false;
m_upAxis = upAxis;
m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
// determine min/max axis-aligned bounding box (aabb) values
switch (m_upAxis)
{
case 0:
{
m_localAabbMin.setValue(m_minHeight, 0, 0);
m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
break;
}
case 1:
{
m_localAabbMin.setValue(0, m_minHeight, 0);
m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
break;
};
case 2:
{
m_localAabbMin.setValue(0, 0, m_minHeight);
m_localAabbMax.setValue(m_width, m_length, m_maxHeight);
break;
}
default:
{
//need to get valid m_upAxis
btAssert(0 && "Bad m_upAxis");
}
}
// remember origin (defined as exact middle of aabb)
m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
}
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_J2linearAxis[i*skip]=0;
info->m_J2linearAxis[i*skip+1]=0;
info->m_J2linearAxis[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)
if (!m_angularOnly)
{
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[skip + 1] = 1;
info->m_J1linearAxis[2 * skip + 2] = 1;
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[skip + 1] = -1;
info->m_J2linearAxis[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 normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
btScalar k = info->fps * normalErp;
if (!m_angularOnly)
{
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())
{
#ifdef _BT_USE_CENTER_LIMIT_
limit_err = m_limit.getCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > btScalar(0.0)) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = getEnableAngularMotor();
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 = false;
}
info->m_constraintError[srow] = btScalar(0.0f);
btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
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)
#ifdef _BT_USE_CENTER_LIMIT_
btScalar bounce = m_limit.getRelaxationFactor();
#else
btScalar bounce = m_relaxationFactor;
#endif
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;
}
}
}
}
#ifdef _BT_USE_CENTER_LIMIT_
info->m_constraintError[srow] *= m_limit.getBiasFactor();
#else
info->m_constraintError[srow] *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
}
