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