void csBulletPivotJoint::SetPosition (const csVector3& position)
{
if (constraint)
{
constraint->setPivotB (CSToBullet (position, dynSys->internalScale));
body->body->forceActivationState (ACTIVE_TAG);
}
}
void csBulletRigidBody::SetLinearVelocity (const csVector3& vel)
{
linearVelocity = vel;
if (!btBody)
return;
if (physicalState == CS::Physics::STATE_DYNAMIC)
{
btBody->setLinearVelocity (CSToBullet (vel, system->getInternalScale ()));
btBody->activate ();
}
}
HeightMapCollider::HeightMapCollider (csBulletDynamicsSystem* dynSys,
iBody* csBody,
csLockedHeightData gridData,
int gridWidth, int gridHeight,
csVector3 gridSize,
csOrthoTransform transform,
float minimumHeight, float maximumHeight)
: dynSys (dynSys)
{
// Check if the min/max have to be computed
bool needExtremum = minimumHeight == 0.0f && maximumHeight == 0.0f;
if (needExtremum)
minimumHeight = maximumHeight = gridData.data[0];
// Initialize the terrain height data
heightData = new float[gridHeight * gridWidth];
for (int i = 0; i < gridWidth; i++)
for (int j = 0; j < gridHeight; j++)
{
float height = heightData[(gridWidth - i - 1) * gridWidth + j]
= gridData.data[i * gridWidth + j];
if (needExtremum)
{
minimumHeight = MIN (minimumHeight, height);
maximumHeight = MAX (maximumHeight, height);
}
}
// Create the terrain shape
shape = new btHeightfieldTerrainShape (gridWidth, gridHeight,
heightData, 1.0f, minimumHeight, maximumHeight,
1, PHY_FLOAT, false);
// Apply the local scaling on the shape
btVector3 localScale (gridSize[0] * dynSys->internalScale / (gridWidth - 1),
dynSys->internalScale,
gridSize[2] * dynSys->internalScale / (gridHeight - 1));
shape->setLocalScaling (localScale);
// Set the origin to the middle of the heightfield
csVector3 offset (gridSize[0] * 0.5f,
(maximumHeight - minimumHeight) * 0.5f + minimumHeight,
gridSize[2] * 0.5f);
transform.SetOrigin (transform.GetOrigin () + transform.This2OtherRelative (offset));
btTransform tr = CSToBullet (transform, dynSys->internalScale);
// Create the rigid body and add it to the world
body = new btRigidBody (0, 0, shape, btVector3 (0, 0, 0));
body->setWorldTransform (tr);
body->setUserPointer (csBody);
dynSys->bulletWorld->addRigidBody (body);
}
void csBulletPivotJoint::Attach (::iRigidBody* body,
const csVector3& position)
{
CS_ASSERT (body);
this->body = static_cast<csBulletRigidBody*> ((csBulletRigidBody*) body);
CS_ASSERT (this->body);
btVector3 localPivot = this->body->body->getCenterOfMassTransform ().inverse ()
* CSToBullet (position, dynSys->internalScale);
constraint = new btPoint2PointConstraint
(*this->body->body, localPivot);
dynSys->bulletWorld->addConstraint (constraint);
constraint->m_setting.m_tau = 0.1f;
}
bool csBulletCollider::CreateBoxGeometry (const csVector3& size)
{
delete shape;
shape = 0;
delete[] vertices;
vertices = 0;
delete[] indices;
indices = 0;
shape = new btBoxShape (CSToBullet (size * 0.5f, dynSys->internalScale));
geomType = BOX_COLLIDER_GEOMETRY;
if (isStaticBody)
{
body->compoundChanged = true;
body->RebuildBody ();
}
return true;
}
bool csBulletJoint::RebuildJoint ()
{
// TODO: use (or deprecate?) angular_constraints_axis
// TODO: use btGeneric6DofSpringConstraint if there is some stiffness
// TODO: use m_damping
// TODO:: allow setting a motor with a target velocity of 0
if (constraint)
{
dynSys->bulletWorld->removeConstraint (constraint);
delete constraint;
constraint = 0;
}
if (!bodies[0] || !bodies[1]) return false;
if (!bodies[0]->body || !bodies[1]->body) return false;
jointType = ComputeBestBulletJointType ();
switch (jointType)
{
case BULLET_JOINT_6DOF:
{
// compute local transforms of the joint
btTransform jointTransform =
CSToBullet (transform * bodies[1]->GetTransform (), dynSys->internalScale);
btTransform frA;
frA = bodies[0]->body->getCenterOfMassTransform().inverse() * jointTransform;
// TODO: for some reason, the joint will be misplaced if some of the bodies are static
btTransform frB;
frB = bodies[1]->body->getCenterOfMassTransform().inverse() * jointTransform;
// create joint
btGeneric6DofConstraint* dof6 =
new btGeneric6DofConstraint (*bodies[0]->body, *bodies[1]->body,
frA, frB, true);
// compute min/max values
btVector3 minLinear(0.0f, 0.0f, 0.0f);
btVector3 maxLinear(0.0f, 0.0f, 0.0f);
if (!trans_constraint_x)
{
minLinear.setX(min_dist[0]);
maxLinear.setX(max_dist[0]);
}
if (!trans_constraint_y)
{
minLinear.setY(min_dist[1]);
maxLinear.setY(max_dist[1]);
}
if (!trans_constraint_z)
{
minLinear.setZ(min_dist[2]);
maxLinear.setZ(max_dist[2]);
}
btVector3 minAngular(0.0f, 0.0f, 0.0f);
btVector3 maxAngular(0.0f, 0.0f, 0.0f);
if (!rot_constraint_x)
{
minAngular.setX(min_angle[0]);
maxAngular.setX(max_angle[0]);
}
if (!rot_constraint_y)
{
minAngular.setY(min_angle[1]);
maxAngular.setY(max_angle[1]);
}
if (!rot_constraint_z)
{
minAngular.setZ(min_angle[2]);
maxAngular.setZ(max_angle[2]);
}
// apply min/max values
dof6->setLinearLowerLimit (minLinear);
dof6->setLinearUpperLimit (maxLinear);
dof6->setAngularLowerLimit (minAngular);
dof6->setAngularUpperLimit (maxAngular);
// apply the parameters for the motor
if (fabs (desired_velocity[0]) > EPSILON)
{
btRotationalLimitMotor* motor = dof6->getRotationalLimitMotor (0);
motor->m_enableMotor = true;
motor->m_targetVelocity = desired_velocity[0];
motor->m_maxMotorForce = maxforce[0];
}
dof6->getRotationalLimitMotor (0)->m_bounce = bounce[0];
if (fabs (desired_velocity[1]) > EPSILON)
{
printf ("Setting motor\n");
btRotationalLimitMotor* motor = dof6->getRotationalLimitMotor (1);
motor->m_enableMotor = true;
motor->m_targetVelocity = desired_velocity[1];
motor->m_maxMotorForce = maxforce[1];
motor->m_damping = 0.1f;
}
dof6->getRotationalLimitMotor (1)->m_bounce = bounce[1];
if (fabs (desired_velocity[2]) > EPSILON)
{
btRotationalLimitMotor* motor = dof6->getRotationalLimitMotor (2);
motor->m_enableMotor = true;
motor->m_targetVelocity = desired_velocity[2];
motor->m_maxMotorForce = maxforce[2];
motor->m_damping = 0.1f;
}
dof6->getRotationalLimitMotor (2)->m_bounce = bounce[2];
constraint = dof6;
}
break;
case BULLET_JOINT_CONETWIST:
{
// compute local transforms of the joint
btTransform frA;
btTransform frB;
btTransform jointTransform = CSToBullet (bodies[1]->GetTransform (),
dynSys->internalScale);
bodies[0]->motionState->getWorldTransform (frA);
frA = frA.inverse () * jointTransform;
bodies[1]->motionState->getWorldTransform (frB);
frB = frB.inverse () * jointTransform;
// create joint
btConeTwistConstraint* coneTwist = new btConeTwistConstraint (*bodies[0]->body, *bodies[1]->body,
frA, frB);
// apply min/max values
if (min_angle[0] < max_angle[0])
coneTwist->setLimit (3, (max_angle[0] - min_angle[0]) * 5.0f);
if (min_angle[1] < max_angle[1])
coneTwist->setLimit (4, (max_angle[1] - min_angle[1]) * 5.0f);
if (min_angle[2] < max_angle[2])
coneTwist->setLimit (5, (max_angle[2] - min_angle[2]) * 5.0f);
constraint = coneTwist;
}
break;
case BULLET_JOINT_POINT2POINT:
{
// TODO
}
break;
default:
// @@@ TODO
break;
}
if (constraint)
{
dynSys->bulletWorld->addConstraint (constraint, true);
return true;
}
return false;
}
void csBulletJoint::SetMaximumDistance (const csVector3& max, bool force_update)
{
max_dist = CSToBullet (max, dynSys->internalScale);
if (force_update)
RebuildJoint ();
}
bool csBulletRigidBody::SetState (CS::Physics::RigidBodyState state)
{
if (physicalState != state || !insideWorld)
{
CS::Physics::RigidBodyState previousState = physicalState;
physicalState = state;
if (!btBody)
return false;
if (haveStaticColliders > 0 && state != CS::Physics::STATE_STATIC)
return false;
if (insideWorld)
sector->bulletWorld->removeRigidBody (btBody);
btVector3 linearVelo = CSToBullet (linearVelocity, system->getInternalScale ());
btVector3 angularVelo = btVector3 (angularVelocity.x, angularVelocity.y, angularVelocity.z);
if (previousState == CS::Physics::STATE_KINEMATIC && insideWorld)
{
btBody->setCollisionFlags (btBody->getCollisionFlags()
& ~btCollisionObject::CF_KINEMATIC_OBJECT);
linearVelo = btBody->getInterpolationLinearVelocity ();
angularVelo = btBody->getInterpolationAngularVelocity ();
// create new motion state
btTransform principalAxis = motionState->inversePrincipalAxis.inverse ();
btTransform trans;
motionState->getWorldTransform (trans);
trans = trans * motionState->inversePrincipalAxis;
delete motionState;
motionState = new csBulletMotionState
(this, trans * principalAxis, principalAxis);
btBody->setMotionState (motionState);
}
if (state == CS::Physics::STATE_DYNAMIC)
{
btBody->setCollisionFlags (btBody->getCollisionFlags()
& ~btCollisionObject::CF_STATIC_OBJECT);
float mass = GetMass ();
btVector3 localInertia (0.0f, 0.0f, 0.0f);
if (compoundShape)
compoundShape->calculateLocalInertia (mass, localInertia);
else
colliders[0]->shape->calculateLocalInertia (mass, localInertia);
btBody->setMassProps (mass, localInertia);
btBody->forceActivationState (ACTIVE_TAG);
btBody->setLinearVelocity (linearVelo);
btBody->setAngularVelocity (angularVelo);
btBody->updateInertiaTensor ();
}
else if (state == CS::Physics::STATE_KINEMATIC)
{
if (!kinematicCb)
kinematicCb.AttachNew (new csBulletDefaultKinematicCallback ());
// create new motion state
btTransform principalAxis = motionState->inversePrincipalAxis.inverse ();
btTransform trans;
motionState->getWorldTransform (trans);
delete motionState;
motionState = new csBulletKinematicMotionState
(this, trans, principalAxis);
btBody->setMotionState (motionState);
// set body kinematic
btBody->setMassProps (0.0f, btVector3 (0.0f, 0.0f, 0.0f));
btBody->setCollisionFlags (btBody->getCollisionFlags()
| btCollisionObject::CF_KINEMATIC_OBJECT
& ~btCollisionObject::CF_STATIC_OBJECT);
btBody->setActivationState (DISABLE_DEACTIVATION);
btBody->updateInertiaTensor ();
btBody->setInterpolationWorldTransform (btBody->getWorldTransform ());
btBody->setInterpolationLinearVelocity (btVector3(0.0f, 0.0f, 0.0f));
btBody->setInterpolationAngularVelocity (btVector3(0.0f, 0.0f, 0.0f));
}
else if (state == CS::Physics::STATE_STATIC)
{
btBody->setCollisionFlags (btBody->getCollisionFlags()
| btCollisionObject::CF_STATIC_OBJECT
& ~btCollisionObject::CF_KINEMATIC_OBJECT);
btBody->setActivationState (ISLAND_SLEEPING);
btBody->setMassProps (0.0f, btVector3 (0.0f, 0.0f, 0.0f));
btBody->updateInertiaTensor ();
}
if (insideWorld)
sector->bulletWorld->addRigidBody (btBody);
return true;
}
else
return false;
}
bool csBulletRigidBody::AddBulletObject ()
{
// TODO: don't recompute everything if the internal scale hasn't changed
if (insideWorld)
RemoveBulletObject ();
btVector3 localInertia (0.0f, 0.0f, 0.0f);
float mass = GetMass ();
btCollisionShape* shape;
btTransform principalAxis;
principalAxis.setIdentity ();
btVector3 principalInertia(0,0,0);
//Create btRigidBody
if (compoundShape)
{
int shapeCount = compoundShape->getNumChildShapes ();
CS_ALLOC_STACK_ARRAY(btScalar, masses, shapeCount);
for (int i = 0; i < shapeCount; i++)
masses[i] = density * colliders[i]->GetVolume ();
if (shapeChanged)
{
compoundShape->calculatePrincipalAxisTransform
(masses, principalAxis, principalInertia);
// apply principal axis
// creation is faster using a new compound to store the shifted children
btCompoundShape* newCompoundShape = new btCompoundShape();
for (int i = 0; i < shapeCount; i++)
{
btTransform newChildTransform =
principalAxis.inverse() * compoundShape->getChildTransform (i);
newCompoundShape->addChildShape(newChildTransform,
compoundShape->getChildShape (i));
shapeChanged = false;
}
delete compoundShape;
compoundShape = newCompoundShape;
}
shape = compoundShape;
}
else
{
shape = colliders[0]->shape;
principalAxis = CSToBullet (relaTransforms[0], system->getInternalScale ());
}
// create new motion state
btTransform trans;
motionState->getWorldTransform (trans);
trans = trans * motionState->inversePrincipalAxis;
delete motionState;
motionState = new csBulletMotionState (this, trans * principalAxis,
principalAxis);
//shape->calculateLocalInertia (mass, localInertia);
btRigidBody::btRigidBodyConstructionInfo infos (mass, motionState,
shape, localInertia);
infos.m_friction = friction;
infos.m_restitution = elasticity;
infos.m_linearDamping = linearDampening;
infos.m_angularDamping = angularDampening;
// create new rigid body
btBody = new btRigidBody (infos);
btObject = btBody;
if (haveStaticColliders > 0)
physicalState = CS::Physics::STATE_STATIC;
SetState (physicalState);
sector->bulletWorld->addRigidBody (btBody, collGroup.value, collGroup.mask);
btBody->setUserPointer (dynamic_cast<CS::Collisions::iCollisionObject*> (
dynamic_cast<csBulletCollisionObject*>(this)));
insideWorld = true;
return true;
}
