void Physics::updateKinematics(float delta){
Vector displacement=delta*velocity_+0.5f*delta*delta*acceleration_;
bv_->translate(displacement.x,displacement.y,displacement.z);
velocity_+=(delta*acceleration());
Matrix omega( 0, angVel_.z, -angVel_.y, 0,
-angVel_.z, 0, angVel_.x, 0,
angVel_.y, -angVel_.x, 0, 0,
0,0,0,0); //set up the matrix for calculating rotation based on
//the objects angular velocity
Matrix Rcurr= bv_->rotation(); //Get the current rotation of the object
Matrix Rdelta=delta*(omega*Rcurr); //Calculate the change in rotation of the object
Matrix Rnew=orthoNormalize(Rcurr+Rdelta); //calculate the new rotation matrix
Matrix Rnet=orthoNormalize(Rcurr.transpose()*Rnew); //figure out what the net rotation is
angVel_=angVel_+delta*angAccel_; //update the angular velocity
bv_->rotate(Rnet);
}
// uses simple explicit euler integration
void RigidBody::integrate(float delta_time) {
//printf("integrate %f\n", delta_time);
float inv_mass = 1.0f/mass;
position += delta_time * velocity;
#if 0
mat3 skew_mat = transpose(skewSymmetric(angular_velocity));
orientation = orientation + delta_time * (skew_mat*orientation);
orientation = orthoNormalize(orientation);
#endif
// integrate quaternion orientation
quat spin = 0.5f * quaternion(0.0f, angular_velocity.x, angular_velocity.y, angular_velocity.z) * q_orientation;
q_orientation += delta_time * spin;
q_orientation = normalize(q_orientation);
velocity += /*0.5f */ (delta_time*inv_mass) * force;
angular_momentum += /*0.5f */ delta_time * torque;
// compute auxiliaries
#if 1
orientation = m3(q_orientation);
inv_world_inertia_mat = orientation * inv_inertia_mat * transpose(orientation);
angular_velocity = inv_world_inertia_mat * angular_momentum;
#else
inv_world_inertia_mat = inv_inertia_mat;
#endif
angular_velocity = inv_world_inertia_mat * angular_momentum;
force = v3(0.0f);
torque = v3(0.0f);
}
void Cutscene::onUpdateActivity(float dt)
{
assert( _clump );
// update animation
_clump->getAnimationController()->advance( dt );
// if there is end of animation sequence
if( _clump->getAnimationController()->isEndOfAnimation( 0 ) )
{
// look for another sequence
if( _animSequenceI != NULL &&
_animSequenceI != _animSequenceL.end() )
{
_animSequenceI++;
if( _animSequenceI == _animSequenceL.end() )
{
_animSequenceI = _animSequenceL.begin();
}
_clump->getAnimationController()->setTrackAnimation( 0, &(*_animSequenceI) );
_clump->getAnimationController()->setTrackSpeed( 0, _desc.speed );
_clump->getAnimationController()->resetTrackTime( 0 );
}
}
// search for camera tracker
engine::IFrame* frame = Gameplay::iEngine->findFrame( _clump->getFrame(), "Camera" ); assert( frame );
_clump->getFrame()->getLTM();
Matrix4f cameraLTM = frame->getLTM();
orthoNormalize( cameraLTM );
// setup camera pose
_camera->setMatrix( cameraLTM );
// wait for any key
bool anykey = false;
input::KeyboardState* keyboardState = Gameplay::iGameplay->getKeyboardState();
for( unsigned int i=0; i<255; i++ )
{
if( keyboardState->keyState[i] & 0x80 )
{
anykey = true;
break;
}
}
if( anykey )
{
new MissionBrowser( _scene, _camera->getPose(), 60 * CAMERA_FOV_MULTIPLIER );
}
}
void updateMatrix(int * accel, int * gyro, int* matrix){
int i;
int normaccel[3];
int wAccel[3];
int orient[3];
int output[9];
normalize(accel, normaccel);
cross(&matrix[6], normaccel, wAccel);
for(i = 0; i < 3; i ++){
orient[i] = ((gyro[i] * GYROSCALE) + (wAccel[i] * ACCELSCALE))/(DIVFACTOR);
}
for(i = 0; i < 3; i ++){
cross(&matrix[3 * i], orient, &output[i * 3]);
vectorAdd(&matrix[3 * i], &output[i * 3], &matrix[3 * i]);
}
orthoNormalize(matrix);
}
// construct a unit quaternion from a rotation matrix
Quaternion::Quaternion(const Matrix4x4 &m) {
float ortho[4][4];
memcpy(ortho, m.m, sizeof(float) * 16);
orthoNormalize(ortho);
const float trace = ortho[0][0] + ortho[1][1] + ortho[2][2] + 1.f;
if (trace > 1e-6f) {
const float s = sqrtf(trace) * 2.f;
v.x = (ortho[1][2] - ortho[2][1]) / s;
v.y = (ortho[2][0] - ortho[0][2]) / s;
v.z = (ortho[0][1] - ortho[1][0]) / s;
w = 0.25f * s;
} else {
if ( ortho[0][0] > ortho[1][1] && ortho[0][0] > ortho[2][2] ) {
// Column 0:
const float s = sqrtf(1.f + ortho[0][0] - ortho[1][1] - ortho[2][2]) * 2.f;
v.x = 0.25f * s;
v.y = (ortho[0][1] + ortho[1][0] ) / s;
v.z = (ortho[2][0] + ortho[0][2] ) / s;
w = (ortho[1][2] - ortho[2][1] ) / s;
} else if ( ortho[1][1] > ortho[2][2] ) {
// Column 1:
const float s = sqrtf(1.f + ortho[1][1] - ortho[0][0] - ortho[2][2]) * 2.f;
v.x = (ortho[0][1] + ortho[1][0] ) / s;
v.y = 0.25f * s;
v.z = (ortho[1][2] + ortho[2][1] ) / s;
w = (ortho[2][0] - ortho[0][2] ) / s;
} else {
// Column 2:
const float s = sqrtf(1.f + ortho[2][2] - ortho[0][0] - ortho[1][1]) * 2.f;
v.x = (ortho[2][0] + ortho[0][2] ) / s;
v.y = (ortho[1][2] + ortho[2][1] ) / s;
v.z = 0.25f * s;
w = (ortho[0][1] - ortho[1][0] ) / s;
}
}
}
void Jumper::AirplaneJump::update(float dt)
{
updateAnimation( dt );
bool useWingsuit = database::Suit::getRecord( _jumper->getVirtues()->equipment.suit.id )->wingsuit;
float phaseTime = useWingsuit ? trackInvSpeed * ( FRAMETIME(2113) - FRAMETIME(2104) ) : trackInvSpeed * ( FRAMETIME(1673) - FRAMETIME(1669) );
if( _actionTime > _blendTime + phaseTime )
{
if( _phActor->isSleeping() )
{
// place jumper to airplane exit
Matrix4f clumpLTM = _clump->getFrame()->getLTM();
Vector3f clumpScale = calcScale( clumpLTM );
Matrix4f exitLTM = _jumper->getAirplaneExit()->getLTM();
orthoNormalize( exitLTM );
exitLTM[0][0] *= clumpScale[0], exitLTM[0][1] *= clumpScale[0], exitLTM[0][2] *= clumpScale[0];
exitLTM[1][0] *= clumpScale[1], exitLTM[1][1] *= clumpScale[1], exitLTM[1][2] *= clumpScale[1];
exitLTM[2][0] *= clumpScale[2], exitLTM[2][1] *= clumpScale[2], exitLTM[2][2] *= clumpScale[2];
_clump->getFrame()->setMatrix( exitLTM );
_clump->getFrame()->getLTM();
Matrix4f sampleLTM = Jumper::getCollisionFF( _clump )->getFrame()->getLTM();
_phActor->setGlobalPose( wrap( sampleLTM ) );
_phActor->wakeUp();
NxVec3 velH = wrap( _clump->getFrame()->getAt() );
velH.normalize();
velH *= 3.0f;
NxVec3 velV = wrap( _clump->getFrame()->getUp() );
velV.normalize();
velV *= 0.25f;
NxVec3 velA = wrap( _jumper->getAirplane()->getVel() );
_phActor->setLinearVelocity( velH + velV + velA );
_jumper->initOverburdenCalculator( velH + velV + velA );
// modified exits (only fixed wing, not heli)
bool helicopter = strcmp(_jumper->getAirplane()->getDesc()->templateClump->getName(), "Helicopter01") == 0;
if (!helicopter) {
if (_jumper->getSpinalCord()->left) {
_phActor->addLocalTorque(NxVec3(0,5700.0f,0));
//_phActor->setAngularDamping(2.0f);
} else if (_jumper->getSpinalCord()->right) {
_phActor->addLocalTorque(NxVec3(0,-5700.0f,0));
//_phActor->setAngularDamping(2.0f);
}
if (_jumper->getSpinalCord()->up) { // headdown exit
_phActor->addLocalTorque(NxVec3(5700.0f,0,0));
} else if (_jumper->getSpinalCord()->down) { // sitfly exit
_phActor->addLocalTorque(NxVec3(-8700.0f,0,0));
_phActor->addLocalForce(NxVec3(0,0,10000.0f));
}
_phActor->setAngularDamping(2.0f);
}
}
else
{
if (_jumper->getSpinalCord()->left) {
_phActor->addLocalTorque(NxVec3(0,2000.0f*dt,0));
} else if (_jumper->getSpinalCord()->right) {
_phActor->addLocalTorque(NxVec3(0,-2000.0f*dt,0));
}
_clump->getFrame()->setMatrix( _matrixConversion->convert( wrap( _phActor->getGlobalPose() ) ) );
}
}
else
{
// place jumper to airplane exit
Matrix4f clumpLTM = _clump->getFrame()->getLTM();
Vector3f clumpScale = calcScale( clumpLTM );
Matrix4f exitLTM = _jumper->getAirplaneExit()->getLTM();
Vector3f pos = _jumper->getAirplaneExit()->getPos();
getCore()->logMessage("exit pos: %2.2f %2.2f %2.2f", pos[0], pos[1], pos[2]);
orthoNormalize( exitLTM );
exitLTM[0][0] *= clumpScale[0], exitLTM[0][1] *= clumpScale[0], exitLTM[0][2] *= clumpScale[0];
exitLTM[1][0] *= clumpScale[1], exitLTM[1][1] *= clumpScale[1], exitLTM[1][2] *= clumpScale[1];
exitLTM[2][0] *= clumpScale[2], exitLTM[2][1] *= clumpScale[2], exitLTM[2][2] *= clumpScale[2];
_clump->getFrame()->setMatrix( exitLTM );
}
if( _clump->getAnimationController()->isEndOfAnimation( 0 )) // || (_jumper->getSpinalCord()->modifier && _jumper->isPlayer() && _actionTime > _blendTime + phaseTime ))
{
_endOfAction = true;
}
}
