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;
}
void CPlayerController::do_simulation_controller( IVP_Event_Sim *es,IVP_U_Vector<IVP_Core> *)
{
if ( !m_enable )
return;
IVP_Real_Object *pivp = m_pObject->GetObject();
IVP_Environment *pIVPEnv = pivp->get_environment();
CPhysicsEnvironment *pVEnv = (CPhysicsEnvironment *)pIVPEnv->client_data;
float psiScale = pVEnv->GetInvPSIScale();
if ( !psiScale )
return;
IVP_Core *pCore = pivp->get_core();
// current situation
const IVP_U_Matrix *m_world_f_core = pCore->get_m_world_f_core_PSI();
const IVP_U_Point *cur_pos_ws = m_world_f_core->get_position();
// ---------------------------------------------------------
// Translation
// ---------------------------------------------------------
IVP_U_Float_Point delta_position; delta_position.subtract( &m_targetPosition, cur_pos_ws);
if (!pivp->flags.shift_core_f_object_is_zero)
{
IVP_U_Float_Point shift_cs_os_ws;
m_world_f_core->vmult3( pivp->get_shift_core_f_object(), &shift_cs_os_ws);
delta_position.subtract( &shift_cs_os_ws );
}
IVP_DOUBLE qdist = delta_position.quad_length();
// UNDONE: This is totally bogus! Measure error using last known estimate
// not current position!
if ( qdist > m_maxDeltaPosition * m_maxDeltaPosition )
{
if ( TryTeleportObject() )
return;
}
// float to allow stepping
if ( m_onground )
{
const IVP_U_Point *pgrav = es->environment->get_gravity();
IVP_U_Float_Point gravSpeed;
gravSpeed.set_multiple( pgrav, es->delta_time );
pCore->speed.subtract( &gravSpeed );
}
ComputeController( pCore->speed, delta_position, m_maxSpeed, psiScale / es->delta_time, m_dampFactor );
}
void CPlayerController::CalculateVelocity(float dt) {
btRigidBody *body = m_pObject->GetObject();
m_secondsToArrival -= dt;
if (m_secondsToArrival < 0) m_secondsToArrival = 0;
float psiScale = m_pEnv->GetInvPSIScale();
btTransform transform;
((btMassCenterMotionState *)body->getMotionState())->getGraphicTransform(transform);
btVector3 deltaPos = m_targetPosition - transform.getOrigin();
ComputeController(m_linVelocity, deltaPos, m_maxSpeed, SAFE_DIVIDE(psiScale, dt), m_dampFactor);
// Apply gravity velocity for stepping.
/*
if (m_onground) {
btVector3 gravVel = body->getGravity() * dt;
m_linVelocity += gravVel;
}
*/
}
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;
}
float ComputeShadowControllerBull(btRigidBody *object, shadowcontrol_params_t ¶ms, float secondsToArrival, float dt) {
// Fraction of the movement we need to complete by this tick
float fraction = 1;
if (secondsToArrival > 0) {
fraction = dt / secondsToArrival;
if (fraction > 1) fraction = 1;
}
secondsToArrival -= dt;
if (secondsToArrival < 0) secondsToArrival = 0;
if (fraction <= 0) return secondsToArrival;
float scale = SAFE_DIVIDE(fraction, dt);
btTransform transform = object->getWorldTransform();
transform *= ((btMassCenterMotionState *)object->getMotionState())->m_centerOfMassOffset.inverse();
//-------------------
// Translation
//-------------------
btVector3 posbull = transform.getOrigin();
btVector3 delta_position = params.targetPosition - posbull;
// Teleportation
// If our distance is greater than teleport distance, teleport instead.
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);
object->setWorldTransform(transform * ((btMassCenterMotionState *)object->getMotionState())->m_centerOfMassOffset);
}
}
btVector3 speed = object->getLinearVelocity();
ComputeController(speed, delta_position, btVector3(params.maxSpeed, params.maxSpeed, params.maxSpeed), scale, params.dampFactor);
object->setLinearVelocity(speed);
params.lastPosition = posbull + (speed * dt);
//-------------------
// Rotation
//-------------------
btVector3 axis;
btScalar angle;
btTransformUtil::calculateDiffAxisAngleQuaternion(transform.getRotation(), params.targetRotation, axis, angle);
// So we don't end up having a huge delta angle (such as instead of doing 379 deg turn, do a -1 deg turn)
if (angle > M_PI) {
angle -= btScalar(2 * M_PI);
}
btVector3 deltaAngles = axis * angle;
btVector3 rot_speed = object->getAngularVelocity();
ComputeController(rot_speed, deltaAngles, btVector3(params.maxAngular, params.maxAngular, params.maxAngular), scale, params.dampFactor);
object->setAngularVelocity(rot_speed);
object->activate();
return secondsToArrival;
}
