public virtual Object CastRay(ref IndexedVector3 from, ref IndexedVector3 to, ref VehicleRaycasterResult result)
{
// RayResultCallback& resultCallback;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref from, ref to);
m_dynamicsWorld.RayTest(ref from, ref to, rayCallback);
if (rayCallback.HasHit())
{
RigidBody body = RigidBody.Upcast(rayCallback.m_collisionObject);
if (body != null && body.HasContactResponse())
{
result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
result.m_hitNormalInWorld.Normalize();
result.m_distFraction = rayCallback.m_closestHitFraction;
return(body);
}
}
else
{
ClosestRayResultCallback rayCallback2 = new ClosestRayResultCallback(ref from, ref to);
m_dynamicsWorld.RayTest(ref from, ref to, rayCallback2);
}
rayCallback.Cleanup();
return(null);
}
public override void SynchronizeMotionStates()
{
if (m_synchronizeAllMotionStates)
{
//iterate over all collision objects
int length = m_collisionObjects.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = RigidBody.Upcast(m_collisionObjects[i]);
if (body != null)
{
SynchronizeSingleMotionState(body);
}
}
}
else
{
//iterate over all active rigid bodies
int length = m_nonStaticRigidBodies.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body.IsActive())
{
SynchronizeSingleMotionState(body);
}
}
}
}
public override void SetGravity(ref IndexedVector3 gravity)
{
m_gravity = gravity;
foreach (CollisionObject colObj in m_collisionObjects)
{
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
body.SetGravity(ref gravity);
}
}
}
///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject
public override void RemoveCollisionObject(CollisionObject collisionObject)
{
RigidBody body = RigidBody.Upcast(collisionObject);
if (body != null)
{
RemoveRigidBody(body);
}
else
{
base.RemoveCollisionObject(collisionObject);
}
}
public override void ClearForces()
{
///@todo: iterate over awake simulation islands!
for (int i = 0; i < m_collisionObjects.Count; i++)
{
CollisionObject colObj = m_collisionObjects[i];
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
body.ClearForces();
}
}
}
public override void SynchronizeMotionStates()
{
///@todo: iterate over awake simulation islands!
foreach (CollisionObject colObj in m_collisionObjects)
{
RigidBody body = RigidBody.Upcast(colObj);
if (body != null && body.GetMotionState() != null)
{
if (body.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
body.GetMotionState().SetWorldTransform(body.GetWorldTransform());
}
}
}
}
//response between two dynamic objects without friction, assuming 0 penetration depth
public static float ResolveSingleCollision(
RigidBody body1,
CollisionObject colObj2,
ref IndexedVector3 contactPositionWorld,
ref IndexedVector3 contactNormalOnB,
ContactSolverInfo solverInfo,
float distance)
{
RigidBody body2 = RigidBody.Upcast(colObj2);
IndexedVector3 normal = contactNormalOnB;
IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2 != null?body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
IndexedVector3 vel = vel1 - vel2;
float rel_vel = normal.Dot(ref vel);
float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
float restitution = combinedRestitution * -rel_vel;
float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
float velocityError = -(1.0f + restitution) * rel_vel; // * damping;
float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
float denom1 = body2 != null?body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
float relaxation = 1.0f;
float jacDiagABInv = relaxation / (denom0 + denom1);
float penetrationImpulse = positionalError * jacDiagABInv;
float velocityImpulse = velocityError * jacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;
body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
if (body2 != null)
{
body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
}
return(normalImpulse);
}
public void IntegrateTransforms(float timeStep)
{
IndexedMatrix predictedTrans;
foreach (CollisionObject colObj in m_collisionObjects)
{
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (body.IsActive() && (!body.IsStaticObject()))
{
body.PredictIntegratedTransform(timeStep, out predictedTrans);
body.ProceedToTransform(ref predictedTrans);
}
}
}
}
public override void UpdateAabbs()
{
//IndexedMatrix predictedTrans = IndexedMatrix.Identity;
foreach (CollisionObject colObj in m_collisionObjects)
{
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (body.IsActive() && (!body.IsStaticObject()))
{
IndexedVector3 minAabb;
IndexedVector3 maxAabb;
colObj.GetCollisionShape().GetAabb(colObj.GetWorldTransform(), out minAabb, out maxAabb);
IBroadphaseInterface bp = GetBroadphase();
bp.SetAabb(body.GetBroadphaseHandle(), ref minAabb, ref maxAabb, m_dispatcher1);
}
}
}
}
public virtual void SaveKinematicState(float timeStep)
{
///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
///to switch status _after_ adding kinematic objects to the world
///fix it for Bullet 3.x release
int length = m_collisionObjects.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = RigidBody.Upcast(m_collisionObjects[i]);
if (body != null && body.GetActivationState() != ActivationState.ISLAND_SLEEPING)
{
if (body.IsKinematicObject())
{
//to calculate velocities next frame
body.SaveKinematicState(timeStep);
}
}
}
}
public void PredictUnconstraintMotion(float timeStep)
{
foreach (CollisionObject colObj in m_collisionObjects)
{
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (!body.IsStaticObject())
{
if (body.IsActive())
{
body.ApplyGravity();
body.IntegrateVelocities(timeStep);
body.ApplyDamping(timeStep);
IndexedMatrix temp = body.GetInterpolationWorldTransform();
body.PredictIntegratedTransform(timeStep, out temp);
body.SetInterpolationWorldTransform(ref temp);
}
}
}
}
}
public override bool CheckCollideWithOverride(CollisionObject co)
{
RigidBody otherRb = RigidBody.Upcast(co);
if (otherRb == null)
{
return(true);
}
for (int i = 0; i < m_constraintRefs.Count; ++i)
{
TypedConstraint c = m_constraintRefs[i];
if (c.IsEnabled())
{
if (c.GetRigidBodyA() == otherRb || c.GetRigidBodyB() == otherRb)
{
return(false);
}
}
}
return(true);
}
