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