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; }