C++ RotationMatrix类代码示例

/// \param angles The orientation (unit) vector that you want to reflect
/// \return Euler angles that are reflected about the slope of the plane.
EulerAngles Plane::reflectOrientation(const EulerAngles& angles) const
{
  EulerAngles out;
  Vector3 look, up, right;  
  RotationMatrix rotMat;
    
  // get look and up vector from euler angles
  rotMat.setup(angles); 
  look = rotMat.objectToInertial(Vector3(0.0f,0.0f,1.0f));
  up = rotMat.objectToInertial(Vector3(0.0f,1.0f,0.0f));

  // reflect the look and up vectors over the plane
  look = reflectOrientation(look);
  up = -reflectOrientation(up); 
  
  // calculate right vector
  right = Vector3::crossProduct( up, look);
  right.normalize();
    
  // create a rotation matrix from right, up, and look
  rotMat.m11 = right.x; rotMat.m12 = right.y; rotMat.m13 = right.z;
  rotMat.m21 = up.x; rotMat.m22 = up.y; rotMat.m23 = up.z;
  rotMat.m31 = look.x; rotMat.m32 = look.y; rotMat.m33 = look.z;
  
  // calculate new euler angles from the matrix
  out.fromRotationMatrix(rotMat);

  return out;
}

// Resets camera to behind target object
void TetherCamera::reset() 
{
  Vector3 target;
  float targetHeading;

  assert(m_objects); // object manager doesn't exist
  GameObject* obj = m_objects->getObjectPointer(m_targetObjectID);    
  
  assert(obj); // object to follow doesn't exist  
  targetHeading = obj->getOrientation().heading;
  target = obj->getPosition();

  
  target.y += 3.0f;

  cameraOrient.set(targetHeading, 0.0f,0.0f);

  RotationMatrix cameraMatrix;
  cameraMatrix.setup(cameraOrient);

  Vector3 bOffset(0.0f,0.0f, -maxDist);
  
  Vector3 iOffset = cameraMatrix.objectToInertial(bOffset);
  cameraPos = target + iOffset;
}

RotationMatrix ForwardKinematics::calcChestFeetRotation(const KinematicChain& theKinematicChain)
{
  RotationMatrix calculatedRotation;

  // calculate rotation based on foot - torso transformation
  const Pose3D& footLeft = theKinematicChain.theLinks[KinematicChain::LFoot].M;
  const Pose3D& footRight = theKinematicChain.theLinks[KinematicChain::RFoot].M;
  const Pose3D& body = theKinematicChain.theLinks[KinematicChain::Torso].M;

  // local in chest
  Pose3D localFootLeft(body.local(footLeft));
  Pose3D localFootRight(body.local(footRight));
    
  if(abs(localFootLeft.translation.z - localFootRight.translation.z) < 3.f/* magic number */)
  {
    // use average of the calculated rotation of each leg
    double meanX = (localFootLeft.rotation.getXAngle() + localFootRight.rotation.getXAngle())*0.5;
    double meanY = (localFootLeft.rotation.getYAngle() + localFootRight.rotation.getYAngle())*0.5;

    //calculatedRotation.fromKardanRPY(0.0, meanY, meanX);
    calculatedRotation.rotateX(meanX).rotateY(meanY);
  }
  else if(localFootLeft.translation.z > localFootRight.translation.z)
  {
    // use left foot
    calculatedRotation = localFootLeft.rotation;
  }
  else
  {
    // use right foot
    calculatedRotation = localFootRight.rotation;
  }

  return calculatedRotation.invert();
}//end calcChestFeetRotation

void Matrix4x3::setupParentToLocal(const Vector3& pos, const EulerAngles& orient)
{
	RotationMatrix orientMatrix;
	orientMatrix.setup(orient);

	setupParentToLocal(pos,orientMatrix);
}

//Rotate by a random amount.
void Coordinates::RotateRand
(XYZArray & dest, uint & pStart, uint & pLen,
 const uint m, const uint b, const double max)
{
   //Rotate (-max, max) radians about a uniformly random vector
   //Not uniformly random, but symmetrical wrt detailed balance
	
   RotationMatrix matrix = RotationMatrix::FromAxisAngle(
      prngRef.Sym(max), prngRef.PickOnUnitSphere());

   XYZ center = comRef.Get(m);
   uint stop = 0;
   molRef.GetRange(pStart, stop, pLen, m);
   //Copy coordinates
   CopyRange(dest, pStart, 0, pLen);

   boxDimRef.UnwrapPBC(dest, b, center);
   //Do rotation
   for (uint p = 0; p < pLen; p++) //Rotate each point.
   {
      dest.Add(p, -center);
      dest.Set(p, matrix.Apply(dest.Get(p)));
      dest.Add(p, center);
   }
   boxDimRef.WrapPBC(dest, b);

}

void KickModule::commandLegsRelativeToTorso(float *command_angles, Pose3D left_target, Pose3D right_target, float /*tilt*/, float roll, float /*tilt_roll_factor*/, bool left_compliant, bool right_compliant) {

	RotationMatrix rot;
	RotationMatrix foot_rotation;
	rot.rotateX(roll);
	foot_rotation.rotateX(-roll);
	if (left_compliant) {
		left_target.translation = rot * left_target.translation;
		left_target.rotation = left_target.rotation * foot_rotation;
	}
	if (right_compliant) {
		right_target.translation = rot * right_target.translation;
		right_target.rotation = right_target.rotation * foot_rotation;
	}
	bool isValid = false;

	//cout << "*** command legs relative to torso " << endl;
	//cout << "commandLegsRel() left foot x y z " << left_target.translation.x << ", " << left_target.translation.y << ", " << left_target.translation.z << endl;
	//cout << "left foot rotation x y z " << left_target.rotation.getXAngle() << ", " << left_target.rotation.getYAngle() << ", " << left_target.rotation.getZAngle() << endl;
	//cout << "commandLegsRel() right foot x y z " << right_target.translation.x << ", " << right_target.translation.y << ", " << right_target.translation.z << endl;
	//cout << "right foot rotation x y z " << right_target.rotation.getXAngle() << ", " << right_target.rotation.getXAngle() << ", " << right_target.rotation.getXAngle() << endl;

	isValid = inverse_kinematics_.calcLegJoints(left_target, right_target, command_angles, robot_info_->dimensions_);


	command_angles[LHipYawPitch] = 0.0;
	command_angles[RHipYawPitch] = 0.0;



}

void BulletObject::updateRay()
{
  EulerAngles lookEuler = getOrientation();
  lookEuler.bank = 0.0f;
  RotationMatrix look;
  look.setup(lookEuler);
  m_bulletRay = look.objectToInertial(Vector3(0.0f,0.0f,m_range));
}

void KickEngineData::calcLegJoints(const JointData::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions)
{
  float sign = joint == JointData::LHipYawPitch ? 1.f : -1.f;
  float leg0, leg1, leg2, leg3, leg4, leg5;

  const Vector3<>& footPos = (sign > 0) ? positions[Phase::leftFootTra] : positions[Phase::rightFootTra];
  const Vector3<>& footRotAng = (sign > 0) ? positions[Phase::leftFootRot] : positions[Phase::rightFootRot];

  RotationMatrix rotateBodyTilt = RotationMatrix().rotateX(comOffset.x);
  Vector3<> anklePos = rotateBodyTilt * Vector3<>(footPos.x, footPos.y, footPos.z);
  //we need just the leg length x and y have to stay the same
  anklePos.y = footPos.y;
  anklePos.x = footPos.x;
  // for the translation of the footpos we only need to translate the anklepoint, which is the intersection of the axis leg4 and leg5
  // the rotation of the foot will be made later by rotating the footpos around the anklepoint
  anklePos -= Vector3<>(0.f, sign * (theRobotDimensions.lengthBetweenLegs / 2), -theRobotDimensions.heightLeg5Joint);

  RotationMatrix rotateBecauseOfHip = RotationMatrix().rotateZ(footRotAng.z).rotateX(-sign * pi_4);

  anklePos = rotateBecauseOfHip * anklePos;

  float diagonal = anklePos.abs();

  // upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
  float a1 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength -
              theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength + diagonal * diagonal) /
             (2 * theRobotDimensions.upperLegLength * diagonal);
  //if(abs(a1) > 1.f) OUTPUT(idText, text, "clipped a1");
  a1 = abs(a1) > 1.f ? 0.f : acos(a1);

  float a2 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength +
              theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength - diagonal * diagonal) /
             (2 * theRobotDimensions.upperLegLength * theRobotDimensions.lowerLegLength);
  //if(abs(a2) > 1.f) OUTPUT(idText, text, "clipped a2");
  a2 = abs(a2) > 1.f ? pi : acos(a2);

  leg0 = footRotAng.z * sign;
  leg2 = -a1 - atan2(anklePos.x, Vector2<>(anklePos.y, anklePos.z).abs() * -sgn(anklePos.z));

  leg1 = anklePos.z == 0.0f ? 0.0f : atan(anklePos.y / -anklePos.z) * -sign;
  leg3 = pi - a2;

  RotationMatrix footRot = RotationMatrix().rotateX(leg1 * -sign).rotateY(leg2 + leg3);
  footRot = footRot.invert() * rotateBecauseOfHip;

  leg5 = asin(-footRot[2].y) * sign * -1;
  leg4 = -atan2(footRot[2].x, footRot[2].z) * -1;

  leg4 += footRotAng.y;
  leg5 += footRotAng.x;

  jointRequest.angles[joint] = leg0;
  jointRequest.angles[joint + 1] = -pi_4 + leg1;
  jointRequest.angles[joint + 2] = leg2;
  jointRequest.angles[joint + 3] = leg3;
  jointRequest.angles[joint + 4] = leg4;
  jointRequest.angles[joint + 5] = leg5;
}

void KickEngineData::calcLegJoints(const Joints::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions, const DamageConfigurationBody& theDamageConfigurationBody)
{
  const int sign = joint == Joints::lHipYawPitch ? 1 : -1;

  const Vector3f& footPos = (sign > 0) ? positions[Phase::leftFootTra] : positions[Phase::rightFootTra];
  const Vector3f& footRotAng = (sign > 0) ? positions[Phase::leftFootRot] : positions[Phase::rightFootRot];

  const RotationMatrix rotateBodyTilt = RotationMatrix::aroundX(bodyAngle.x()).rotateY(bodyAngle.y());
  Vector3f anklePos = rotateBodyTilt * (footPos - Vector3f(0.f, 0, -theRobotDimensions.footHeight));
  anklePos -= Vector3f(0.f, sign * (theRobotDimensions.yHipOffset), 0);

  //const RotationMatrix zRot = RotationMatrix::aroundZ(footRotAng.z()).rotateX(sign * pi_4);
  //anklePos = zRot * anklePos;
  const float leg0 = 0;//std::atan2(-anklePos.x(), anklePos.y());
  const float diagonal = anklePos.norm();

  // upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
  float a1 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength -
              theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength + diagonal * diagonal) /
             (2 * theRobotDimensions.upperLegLength * diagonal);
  //if(std::abs(a1) > 1.f) OUTPUT_TEXT("clipped a1");
  a1 = std::abs(a1) > 1.f ? 0.f : std::acos(a1);

  float a2 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength +
              theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength - diagonal * diagonal) /
             (2 * theRobotDimensions.upperLegLength * theRobotDimensions.lowerLegLength);
  //if(std::abs(a2) > 1.f) OUTPUT_TEXT("clipped a2");
  a2 = std::abs(a2) > 1.f ? pi : std::acos(a2);

  const float leg2 = -a1 - std::atan2(anklePos.x(), Vector2f(anklePos.y(), anklePos.z()).norm() * -sgn(anklePos.z()));
  const float leg1 = anklePos.z() == 0.0f ? 0.0f : (std::atan(anklePos.y() / -anklePos.z()));
  const float leg3 = pi - a2;

  const RotationMatrix rotateBecauseOfHip = RotationMatrix::aroundZ(0).rotateX(bodyAngle.x()).rotateY(bodyAngle.y());
  //calculate inverse foot rotation so that they are flat to the ground
  RotationMatrix footRot = RotationMatrix::aroundX(leg1).rotateY(leg2 + leg3);
  footRot = footRot.inverse() /* * zRot*/ * rotateBecauseOfHip;

  //and add additonal foot rotation (which is probably not flat to the ground)
  const float leg4 = std::atan2(footRot(0, 2), footRot(2, 2)) + footRotAng.y();
  const float leg5 = std::asin(-footRot(1, 2)) + footRotAng.x() ;

  jointRequest.angles[joint] = leg0;
  jointRequest.angles[joint + 1] = (/*-pi_4 * sign + */leg1);
  jointRequest.angles[joint + 2] = leg2;
  jointRequest.angles[joint + 3] = leg3;
  jointRequest.angles[joint + 4] = leg4;
  jointRequest.angles[joint + 5] = leg5;

  //quickhack which allows calibration, but works only if the left foot is the support foot in .kmc file
  Vector2f tiltCalibration = currentKickRequest.mirror ? theDamageConfigurationBody.startTiltRight : theDamageConfigurationBody.startTiltLeft;
  if(currentKickRequest.mirror)
    tiltCalibration.x() *= -1.f;
  jointRequest.angles[Joints::lAnkleRoll] += tiltCalibration.x();
  jointRequest.angles[Joints::lAnklePitch] += tiltCalibration.y();
}

/// \param pos Specifies the position of the local space within the
///     parent space.
/// \param orient Specifies the orientation of the local space within
///     the parent space as an Euler angle triplet.
void	Matrix4x3::setupParentToLocal(const Vector3 &pos, const EulerAngles &orient) {

	// Create a rotation matrix.

	RotationMatrix orientMatrix;
	orientMatrix.setup(orient);

	// Setup the 4x3 matrix.

	setupParentToLocal(pos, orientMatrix);
}

bool ViewBikeMath::calcLegJoints(const Vector3<>& footPos, const Vector3<>& footRotAng, const bool& left, const RobotDimensions& robotDimensions, float& leg0, float& leg1, float& leg2, float& leg3, float& leg4, float& leg5)
{
  float sign = (left) ? 1.f : -1.f;
  bool legTooShort = false;

  Vector3<> anklePos; //FLOAT
  anklePos = Vector3<>(footPos.x,  footPos.y, footPos.z + robotDimensions.heightLeg5Joint);

  anklePos -= Vector3<>(0.f, sign * (robotDimensions.lengthBetweenLegs / 2.f), 0.f);

  RotationMatrix rotateBecauseOfHip = RotationMatrix().rotateZ(sign * footRotAng.z).rotateX(-sign * pi_4);
  Vector3<> rotatedFootRotAng;


  anklePos = rotateBecauseOfHip * anklePos;
  leg0 = footRotAng.z;
  rotatedFootRotAng = rotateBecauseOfHip * footRotAng;

  leg2 = -std::atan2(anklePos.x, Vector2<>(anklePos.y, anklePos.z).abs());

  float diagonal = anklePos.abs();

  // upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
  float a1 = (robotDimensions.upperLegLength * robotDimensions.upperLegLength -
              robotDimensions.lowerLegLength * robotDimensions.lowerLegLength + diagonal * diagonal) /
             (2 * robotDimensions.upperLegLength * diagonal);
  a1 = std::abs(a1) > 1.f ? 0 : std::acos(a1);

  float a2 = (robotDimensions.upperLegLength * robotDimensions.upperLegLength +
              robotDimensions.lowerLegLength * robotDimensions.lowerLegLength - diagonal * diagonal) /
             (2 * robotDimensions.upperLegLength * robotDimensions.lowerLegLength);
  const Range<float> clipping(-1.f, 1.f);
  if(!clipping.isInside(a2))
  {
    legTooShort = true;
  }
  a2 = std::abs(a2) > 1.f ? pi : std::acos(a2);

  leg1 = (-sign * std::atan2(anklePos.y, std::abs(anklePos.z))) - (pi / 4);
  leg2 -= a1;
  leg3 = pi - a2;

  RotationMatrix footRot = RotationMatrix().rotateX(leg1 * -sign).rotateY(leg2 + leg3);
  footRot = footRot.invert() * rotateBecauseOfHip;
  leg5 = std::asin(-footRot[2].y) * -sign;
  leg4 = -std::atan2(footRot[2].x, footRot[2].z) * -1;
  leg4 += footRotAng.y;
  leg5 += footRotAng.x;

  return legTooShort;
}

bool KickViewMath::calcLegJoints(const Vector3f& footPos, const Vector3f& footRotAng, const bool& left, const RobotDimensions& robotDimensions,
                            float& leg0, float& leg1, float& leg2, float& leg3, float& leg4, float& leg5)
{
  float sign = (left) ? 1.f : -1.f;
  bool legTooShort = false;

  Vector3f anklePos; //FLOAT
  anklePos = Vector3f(footPos.x(),  footPos.y(), footPos.z() + robotDimensions.footHeight);

  anklePos -= Vector3f(0.f, sign * (robotDimensions.yHipOffset), 0.f);

  const RotationMatrix rotateBecauseOfHip = RotationMatrix::aroundZ(sign * footRotAng.z()).rotateX(-sign * pi_4);
  Vector3f rotatedFootRotAng;

  anklePos = rotateBecauseOfHip * anklePos;
  leg0 = footRotAng.z();
  rotatedFootRotAng = rotateBecauseOfHip * footRotAng;

  leg2 = -std::atan2(anklePos.x(), Vector2f(anklePos.y(), anklePos.z()).norm());

  float diagonal = anklePos.norm();

  // upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
  float a1 = (robotDimensions.upperLegLength * robotDimensions.upperLegLength -
              robotDimensions.lowerLegLength * robotDimensions.lowerLegLength + diagonal * diagonal) /
             (2 * robotDimensions.upperLegLength * diagonal);
  a1 = std::abs(a1) > 1.f ? 0 : std::acos(a1);

  float a2 = (robotDimensions.upperLegLength * robotDimensions.upperLegLength +
              robotDimensions.lowerLegLength * robotDimensions.lowerLegLength - diagonal * diagonal) /
             (2 * robotDimensions.upperLegLength * robotDimensions.lowerLegLength);
  if(!Rangef::OneRange().isInside(a2))
  {
    legTooShort = true;
  }
  a2 = std::abs(a2) > 1.f ? pi : std::acos(a2);

  leg1 = (-sign * std::atan2(anklePos.y(), std::abs(anklePos.z()))) - (pi / 4);
  leg2 -= a1;
  leg3 = pi - a2;

  RotationMatrix footRot = RotationMatrix::aroundX(leg1 * -sign).rotateY(leg2 + leg3);
  footRot = footRot.inverse() * rotateBecauseOfHip;
  leg5 = std::asin(-footRot.col(2).y()) * -sign;
  leg4 = -std::atan2(footRot.col(2).x(), footRot.col(2).z()) * -1;
  leg4 += footRotAng.y();
  leg5 += footRotAng.x();

  return legTooShort;
}

bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, float joint0, bool left, const RobotDimensions& robotDimensions) {
  Pose3D target(position);
  Joint firstJoint(left ? LHipYawPitch : RHipYawPitch);
  const int sign(left ? -1 : 1);
  target.translation.y += robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2 * sign; // translate to origin of leg
  target = Pose3D().rotateZ(joint0 * -sign).rotateX(M_PI_4 * sign).conc(target); // compute residual transformation with fixed joint0

  // use cosine theorem and arctan to compute first three joints
  float length = target.translation.abs();
  float sqrLength = length * length;
  float upperLeg = robotDimensions.values_[RobotDimensions::upperLegLength];
  float sqrUpperLeg = upperLeg * upperLeg;
  float lowerLeg = robotDimensions.values_[RobotDimensions::lowerLegLength];
  float sqrLowerLeg = lowerLeg * lowerLeg;
  float cosUpperLeg = (sqrUpperLeg + sqrLength - sqrLowerLeg) / (2 * upperLeg * length);
  float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg);
  // clip for the case that target position is not reachable
  const Range<float> clipping(-1.0f, 1.0f);
  bool reachable = true;
  if(!clipping.isInside(cosKnee) || clipping.isInside(upperLeg))
  {
    cosKnee = clipping.limit(cosKnee);
    cosUpperLeg = clipping.limit(cosUpperLeg);
    reachable = false;
  }
  float joint1 = target.translation.z == 0.0f ? 0.0f : atanf(target.translation.y / -target.translation.z) * sign;
  float joint2 = -acos(cosUpperLeg);
  joint2 -= atan2(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs() * -sgn(target.translation.z));
  float joint3 = M_PI - acos(cosKnee);
  RotationMatrix beforeFoot = RotationMatrix().rotateX(joint1 * sign).rotateY(joint2 + joint3);
  joint1 -= M_PI_4; // because of the strange hip of Nao

  // compute joints from rotation matrix using theorem of euler angles
  // see http://www.geometrictools.com/Documentation/EulerAngles.pdf
  // this is possible because of the known order of joints (y, x, z) where z is left open and is seen as failure
  RotationMatrix foot = beforeFoot.invert() * target.rotation;
  float joint5 = asin(-foot[2].y) * -sign * -1;
  float joint4 = -atan2(foot[2].x, foot[2].z) * -1;
  //float failure = atan2(foot[0].y, foot[1].y) * sign;

  // set computed joints in jointAngles
  jointAngles[firstJoint + 0] = joint0;
  jointAngles[firstJoint + 1] = joint1;
  jointAngles[firstJoint + 2] = joint2;
  jointAngles[firstJoint + 3] = joint3;
  jointAngles[firstJoint + 4] = joint4;
  jointAngles[firstJoint + 5] = joint5;

  return reachable;
}

bool Ned3DObjectManager::interactPlaneTerrain(PlaneObject &plane, TerrainObject &terrain)
{
  Terrain *terr = terrain.getTerrain();
  if(terr == NULL) return false;

  //test for plane collision with terrain
  Vector3 planePos = plane.getPosition();
  EulerAngles planeOrient = plane.getOrientation();
  Vector3 disp = planePos - disp;
  RotationMatrix planeMatrix;
  planeMatrix.setup(plane.getOrientation()); // get plane's orientation

  float planeBottom = plane.getBoundingBox().min.y;
  float terrainHeight = terr->getHeight(planePos.x,planePos.z);
  if(plane.isPlaneAlive() && planeBottom < terrainHeight)
  { //collision
    Vector3 viewVector = planeMatrix.objectToInertial(Vector3(0,0,1));
    if(viewVector * terr->getNormal(planePos.x,planePos.z) < -0.5f // dot product
      || plane.isCrashing())
    { 
      plane.killPlane();
      int partHndl = gParticle.createSystem("planeexplosion");
      gParticle.setSystemPos(partHndl, plane.getPosition());
      int boomHndl = gSoundManager.requestSoundHandle("Boom.wav");
      int boomInst = gSoundManager.requestInstance(boomHndl);
      if(boomInst != SoundManager::NOINSTANCE)
      {
        gSoundManager.setPosition(boomHndl,boomInst,plane.getPosition());
        gSoundManager.play(boomHndl,boomInst);
        gSoundManager.releaseInstance(boomHndl,boomInst);
      }
      plane.setSpeed(0.0f);
      planePos += 2.0f * viewVector;
      planeOrient.pitch = kPi / 4.0f;
      planeOrient.bank = kPi / 4.0f;
      plane.setOrientation(planeOrient);
    }
    else planePos.y = terrainHeight + planePos.y - planeBottom;
    //plane.setPPosition(planePos);
    return true;
  }
  return false;
}

void PlaneObject::damage(int hp)
{
  // if god mode is on leave 
  if (!takeDamage)
    return;

  m_hp -= hp;
  setTextureAndSmoke(); // change to a texture with more damange on it
        
  if(m_isPlaneAlive && m_hp <= 0)
  {
    m_planeState = PS_CRASHING;
    m_velocity = Vector3::kForwardVector;
    m_eaOrient[0].pitch = degToRad(20);
    RotationMatrix r; r.setup(m_eaOrient[0]);
    m_velocity = r.objectToInertial(m_velocity);
    m_velocity *= m_maxSpeed * m_speedRatio * 20.0f;
  }
}