float ComputeShadowControllerBull(btRigidBody* object, shadowcontrol_params_t ¶ms, float secondsToArrival, float dt) {
float fraction = 1.0;
if (secondsToArrival > 0) {
fraction *= dt / secondsToArrival;
if (fraction > 1) fraction = 1;
}
secondsToArrival -= dt;
if (secondsToArrival < 0) secondsToArrival = 0;
if (fraction <= 0) return secondsToArrival;
btTransform transform;
((btMassCenterMotionState*)object->getMotionState())->getGraphicTransform(transform);
btVector3 posbull = transform.getOrigin();
btVector3 delta_position = params.targetPosition - posbull;
if (params.teleportDistance > 0) {
btScalar qdist;
if (!params.lastPosition.isZero()) {
btVector3 tmpDelta = posbull - params.lastPosition;
qdist = tmpDelta.length2();
} else {
qdist = delta_position.length2();
}
if (qdist > params.teleportDistance * params.teleportDistance) {
transform.setOrigin(params.targetPosition);
transform.setRotation(params.targetRotation);
}
}
float invDt = 1.0f / dt;
btVector3 speed = object->getLinearVelocity();
ComputeController(speed, delta_position, params.maxSpeed, fraction * invDt, params.dampFactor);
object->setLinearVelocity(speed);
params.lastPosition = posbull + speed * dt;
btVector3 deltaAngles;
btQuaternion deltaRotation;
QuaternionDiff(params.targetRotation, transform.getRotation(), deltaRotation);
btVector3 axis = deltaRotation.getAxis();
float angle = deltaRotation.getAngle();
axis.normalize();
deltaAngles.setX(axis.x() * angle);
deltaAngles.setY(axis.y() * angle);
deltaAngles.setZ(axis.z() * angle);
btVector3 rot_speed = object->getAngularVelocity();
ComputeController(rot_speed, deltaAngles, params.maxAngular, fraction * invDt, params.dampFactor);
object->setAngularVelocity(rot_speed);
return secondsToArrival;
}
float ComputeShadowControllerIVP( IVP_Real_Object *pivp, shadowcontrol_params_t ¶ms, float secondsToArrival, float dt )
{
// resample fraction
// This allows us to arrive at the target at the requested time
float fraction = 1.0;
if ( secondsToArrival > 0 )
{
fraction *= dt / secondsToArrival;
if ( fraction > 1 )
{
fraction = 1;
}
}
secondsToArrival -= dt;
if ( secondsToArrival < 0 )
{
secondsToArrival = 0;
}
if ( fraction <= 0 )
return secondsToArrival;
// ---------------------------------------------------------
// Translation
// ---------------------------------------------------------
IVP_U_Point positionIVP;
GetObjectPosition_IVP( positionIVP, pivp );
IVP_U_Float_Point delta_position;
delta_position.subtract( ¶ms.targetPosition, &positionIVP);
// BUGBUG: Save off velocities and estimate final positions
// measure error against these final sets
// also, damp out 100% saved velocity, use max additional impulses
// to correct error and damp out error velocity
// extrapolate position
if ( params.teleportDistance > 0 )
{
IVP_DOUBLE qdist;
if ( !IsZeroVector(params.lastPosition) )
{
IVP_U_Float_Point tmpDelta;
tmpDelta.subtract( &positionIVP, ¶ms.lastPosition );
qdist = tmpDelta.quad_length();
}
else
{
// UNDONE: This is totally bogus! Measure error using last known estimate
// not current position!
qdist = delta_position.quad_length();
}
if ( qdist > params.teleportDistance * params.teleportDistance )
{
if ( pivp->is_collision_detection_enabled() )
{
pivp->enable_collision_detection( IVP_FALSE );
pivp->beam_object_to_new_position( ¶ms.targetRotation, ¶ms.targetPosition, IVP_TRUE );
pivp->enable_collision_detection( IVP_TRUE );
}
else
{
pivp->beam_object_to_new_position( ¶ms.targetRotation, ¶ms.targetPosition, IVP_TRUE );
}
}
}
float invDt = 1.0f / dt;
IVP_Core *pCore = pivp->get_core();
ComputeController( pCore->speed, delta_position, params.maxSpeed, fraction * invDt, params.dampFactor );
params.lastPosition.add_multiple( &positionIVP, &pCore->speed, dt );
IVP_U_Float_Point deltaAngles;
// compute rotation offset
IVP_U_Quat deltaRotation;
QuaternionDiff( params.targetRotation, pCore->q_world_f_core_next_psi, deltaRotation );
// convert offset to angular impulse
Vector axis;
float angle;
QuaternionAxisAngle( deltaRotation, axis, angle );
VectorNormalize(axis);
deltaAngles.k[0] = axis.x * angle;
deltaAngles.k[1] = axis.y * angle;
deltaAngles.k[2] = axis.z * angle;
ComputeController( pCore->rot_speed, deltaAngles, params.maxAngular, fraction * invDt, params.dampFactor );
return secondsToArrival;
}
