//---------------------------------------------------------------------------
const Rvector6 Barycenter::GetMJ2000State(const A1Mjd &atTime)
{
// if it's built-in, get the state from the SpacePoint
if (isBuiltIn)
{
lastStateTime = atTime;
lastState = builtInSP->GetMJ2000State(atTime);
#ifdef DEBUG_BARYCENTER_STATE
MessageInterface::ShowMessage("Computing state for Barycenter %s, whose builtInSP is %s\n",
instanceName.c_str(), (builtInSP->GetName()).c_str());
#endif
return lastState;
}
// otherwise, sum the masses and states
CheckBodies();
#ifdef DEBUG_BARYCENTER
MessageInterface::ShowMessage("Entering BaryCenter::GetMJ2000EqState at time %12.10f\n",
atTime.Get());
#endif
Real bodyMass = 0.0;
Rvector3 bodyPos(0.0,0.0,0.0);
Rvector3 bodyVel(0.0,0.0,0.0);
Real weight = 0.0;
Rvector3 sumMassPos(0.0,0.0,0.0);
Rvector3 sumMassVel(0.0,0.0,0.0);
Rvector6 bodyState;
Real sumMass = GetMass();
for (unsigned int i = 0; i < bodyList.size() ; i++)
{
bodyMass = ((CelestialBody*) (bodyList.at(i)))->GetMass();
bodyState = (bodyList.at(i))->GetMJ2000State(atTime);
bodyPos = bodyState.GetR();
bodyVel = bodyState.GetV();
weight = bodyMass/sumMass;
#ifdef DEBUG_BARYCENTER
MessageInterface::ShowMessage("Mass (and weight) of body %s = %12.10f (%12.10f)\n",
((bodyList.at(i))->GetName()).c_str(), bodyMass, weight);
MessageInterface::ShowMessage(" pos = %s\n", (bodyPos.ToString()).c_str());
MessageInterface::ShowMessage(" vel = %s\n", (bodyVel.ToString()).c_str());
#endif
sumMassPos += (weight * bodyPos);
sumMassVel += (weight * bodyVel);
}
#ifdef DEBUG_BARYCENTER
MessageInterface::ShowMessage("sumMassPos = %s\n",
(sumMassPos.ToString()).c_str());
#endif
lastState.Set(sumMassPos(0), sumMassPos(1), sumMassPos(2),
sumMassVel(0), sumMassVel(1), sumMassVel(2));
lastStateTime = atTime;
return lastState;
}
int psLinearMovement::MoveSprite (float delta)
{
int ret = PS_MOVE_SUCCEED;
csReversibleTransform fulltransf = mesh->GetMovable ()
->GetFullTransform ();
// const csMatrix3& transf = fulltransf.GetT2O ();
// Calculate the total velocity (body and world) in OBJECT space.
csVector3 bodyVel (fulltransf.Other2ThisRelative (velWorld) + velBody);
float local_max_interval =
csMin(csMin((bodyVel.y==0.0f) ? MAX_CD_INTERVAL : ABS (intervalSize.y/bodyVel.y),
(bodyVel.x==0.0f) ? MAX_CD_INTERVAL : ABS (intervalSize.x/bodyVel.x)
),(bodyVel.z==0.0f) ? MAX_CD_INTERVAL : ABS (intervalSize.z/bodyVel.z)
);
// Err on the side of safety (95% error margin)
local_max_interval *= 0.95f;
//printf("local_max_interval=%f, bodyVel is %1.2f, %1.2f, %1.2f\n",local_max_interval, bodyVel.x, bodyVel.y, bodyVel.z);
//printf("velWorld is %1.2f, %1.2f, %1.2f\n", velWorld.x, velWorld.y, velWorld.z);
//printf("velBody is %1.2f, %1.2f, %1.2f\n", velBody.x, velBody.y, velBody.z);
// Sanity check on time interval here. Something is messing it up. -KWF
//local_max_interval = csMax(local_max_interval, 0.1F);
if (colldet)
{
while (delta > local_max_interval)
{
csVector3 oldpos(mesh->GetMovable ()->GetFullTransform ().GetOrigin());
// Perform brutal optimisation here for falling with no obstacles
if(velWorld.y < -20.0f && mesh->GetMovable()->GetSectors()->GetCount() > 0)
{
csVector3 worldVel (fulltransf.This2OtherRelative (velBody) + velWorld);
bool hit = false;
// We check for other meshes at the start and end of the box with radius * 2 to be on the safe side
{
csRef<iMeshWrapperIterator> objectIter = engine->GetNearbyMeshes(mesh->GetMovable()->GetSectors()->Get(0),
oldpos + boundingBox.GetCenter(), boundingBox.GetSize().Norm() * 2);
if(objectIter->HasNext())
hit = true;
}
if(!hit)
{
csRef<iMeshWrapperIterator> objectIter = engine->GetNearbyMeshes(mesh->GetMovable()->GetSectors()->Get(0),
oldpos + worldVel * delta + boundingBox.GetCenter(), boundingBox.GetSize().Norm() * 2);
if(objectIter->HasNext())
hit = true;
}
if(!hit)
{
csOrthoTransform transform_newpos(csMatrix3(), oldpos + worldVel * delta);
csBox3 fullBox(fulltransf.This2OtherRelative(boundingBox.Min()), fulltransf.This2OtherRelative(boundingBox.Max()));
csBox3 newBox( transform_newpos.This2OtherRelative(boundingBox.Min()), transform_newpos.This2OtherRelative(boundingBox.Max()));
fullBox.AddBoundingBox(newBox);
csRef<iMeshWrapperIterator> objectIter = engine->GetNearbyMeshes(mesh->GetMovable()->GetSectors()->Get(0), fullBox);
if(objectIter->HasNext())
hit = true;
}
if(!hit)
local_max_interval = delta;
}
ret = MoveV (local_max_interval);
csVector3 displacement(fulltransf.GetOrigin() - oldpos);
displacement = fulltransf.Other2ThisRelative(displacement);
// Check the invariants still hold otherwise we may jump walls
if(!(fabs(displacement.x) <= intervalSize.x))
{
printf("X (%g) out of bounds when performing CD > %g!\n", fabs(displacement.x), intervalSize.x);
CS_ASSERT(false);
}
if(!(fabs(displacement.z) <= intervalSize.z))
{
printf("Z (%g) out of bounds when performing CD > %g!\n", fabs(displacement.y), intervalSize.y);
CS_ASSERT(false);
}
if(!(fabs(displacement.y) <= intervalSize.y))
{
printf("Y (%g) out of bounds when performing CD > %g!\n", fabs(displacement.z), intervalSize.z);
CS_ASSERT(false);
}
RotateV (local_max_interval);
// We must update the transform after every rotation!
fulltransf = mesh->GetMovable ()->GetFullTransform ();
if (ret == PS_MOVE_FAIL)
return ret;
// The velocity may have changed by now
bodyVel = fulltransf.Other2ThisRelative(velWorld) + velBody;
delta -= local_max_interval;
local_max_interval = csMin(csMin((bodyVel.y==0.0f)
? MAX_CD_INTERVAL
: ABS (intervalSize.y/bodyVel.y), (bodyVel.x==0.0f)
? MAX_CD_INTERVAL
: ABS (intervalSize.x/bodyVel.x)), (bodyVel.z==0.0f)
? MAX_CD_INTERVAL
: ABS (intervalSize.z/bodyVel.z));
// Err on the side of safety (95% error margin)
local_max_interval *= 0.95f;
// Sanity check on time interval here. Something is messing it up. -KWF
// local_max_interval = csMax(local_max_interval, 0.1F);
}
}
if (!colldet || delta)
{
ret = MoveV (delta);
RotateV(delta);
}
return ret;
}
