private Quaternion GetRotation(Vector3 src, Vector3 dest)
{
src.Normalize();
dest.Normalize();
float d = Vector3.Dot(src, dest);
if (d >= 1f)
{
return Quaternion.Identity;
}
else if (d < (1e-6f - 1.0f))
{
Vector3 axis = Vector3.Cross(Vector3.UnitX, src);
if (axis.LengthSquared() == 0)
{
axis = Vector3.Cross(Vector3.UnitY, src);
}
axis.Normalize();
return Quaternion.CreateFromAxisAngle(axis, MathHelper.Pi);
}
else
{
float s = (float)Math.Sqrt((1 + d) * 2);
float invS = 1 / s;
Vector3 c = Vector3.Cross(src, dest);
Quaternion q = new Quaternion(invS * c, 0.5f * s);
q.Normalize();
return q;
}
}
/// <summary>
/// Construct a new transform.
/// </summary>
/// <param name="scale">The change in scale multiplier.</param>
/// <param name="position">The change in position.</param>
/// <param name="orientation">The transform's orientation.</param>
public Transform(float scale, ref Vector3 position, ref Quaternion orientation)
{
Scale = scale;
Position = position;
Orientation = orientation;
Orientation.Normalize();
Combine(Scale, ref Position, ref Orientation, out Combined);
}
public void RotateGameObject(ref GameObject toBeRotated)
{
Vector3 axis = toBeRotated.RotationAxis;
float angle = MathHelper.ToRadians(toBeRotated.RotationAngleInDegrees);
float angleHalf = angle / 2;
float sinAngleHalf = (float)Math.Sin(angleHalf);
Quaternion rotationQuaternion = new Quaternion(axis.X * sinAngleHalf, axis.Y * sinAngleHalf, axis.Z * sinAngleHalf, (float)Math.Cos(angleHalf));
rotationQuaternion.Normalize();
Matrix rotationMatrix = Matrix.CreateFromQuaternion(rotationQuaternion);
toBeRotated.World *= rotationMatrix;
}
public PlayerShip(List<Collidable> collidableRef, CollidableType type, float boundingRadius, Model model, Vector3 shipPosition, GraphicsDevice device, Vector3 cameraFollowPosition, float scale, float minShipSpeed, float maxShipSpeed, float acceleration, float turningSpeed,
float maxTurningSpeed, float minTurningSpeed, WeaponSystem2D weaponSystem2D, MissleSystem2D missleSystem2D, SpawnerManager spawnerManager, PlayerConfig[] playerConfigurations)
{
CollidableReference = collidableRef;
_shipModel = new BasicModel(model, scale);
ShipWorld = Matrix.CreateTranslation(shipPosition) * Matrix.CreateScale(scale);
_dockingPosition = shipPosition;
ShipPosition = shipPosition;
_shipRotation = Quaternion.Identity;
Camera = new Camera(device, cameraFollowPosition);
_keyboard = new KeyboardInput();
_minShipSpeed = minShipSpeed;
_maxShipSpeed = maxShipSpeed;
_acceleration = acceleration;
_turningSpeed = turningSpeed;
_gamePadInput = new GamePadInput();
_weaponSystem2D = weaponSystem2D;
_device = device;
_shipRotation.Normalize();
//Collidable Properties
this.CollidableType = type;
this.BoundingSphereRadius = boundingRadius;
_maxTurningSpeed = maxTurningSpeed;
_minTurningSpeed = minTurningSpeed;
Life = 0;
_soundDump = new List<Sound>();
spawnerManager.Selected += (object sender, EventArgs args) =>
{
SpawnerManager SM = sender as SpawnerManager;
Spawn(SM._selected);
};
_missleSystem2D = missleSystem2D;
_playerConfigurations = playerConfigurations;
}
/// <summary>
/// Computes an arc rotation from two vectors
/// </summary>
/// <param name="origin">Origin</param>
/// <param name="destination">Destination</param>
/// <param name="fallback"></param>
/// <param name="rotation">Quaternion containing an arc rotation</param>
public static void GetArcRotation(ref Vector3 origin, ref Vector3 destination, ref Vector3 fallback, out Quaternion rotation)
{
Vector3 start = origin;
Vector3 end = destination;
start.Normalize();
end.Normalize();
float dot;
Vector3.Dot(ref start, ref end, out dot);
if (dot >= 1.0f)
{
rotation = Quaternion.Identity;
return;
}
if (dot < (1e-06f - 1.0f))
{
if (fallback != Vector3.Zero)
Quaternion.CreateFromAxisAngle(ref fallback, MathHelper.ToRadians(MathHelper.Pi), out rotation);
else
{
Vector3 axis = Vector3.Cross(Vector3.UnitZ, start);
if (axis.IsZeroLength()) axis = Vector3.Cross(Vector3.UnitY, start);
axis.Normalize();
Quaternion.CreateFromAxisAngle(ref axis, MathHelper.ToRadians(MathHelper.Pi), out rotation);
}
}
else
{
float s = (float)Math.Sqrt((1.0f + dot) * 2.0f);
float invS = 1.0f / s;
Vector3 cross;
Vector3.Cross(ref start, ref end, out cross);
#if XBOX || XBOX360
rotation = new Quaternion();
#endif
rotation.X = cross.X * invS;
rotation.Y = cross.Y * invS;
rotation.Z = cross.Z * invS;
rotation.W = s * 0.5f;
rotation.Normalize();
}
}
private void HandleUserInput(float dt)
{
Translation = Vector3.Zero;
float deltaX = -_input.Mouse.DeltaX;
float deltaY = -_input.Mouse.DeltaY;
float scale = RotationSpeed * dt;
_rotation = Quaternion.Multiply(Quaternion.CreateFromAxisAngle(Vector3.Up,deltaX * scale),Quaternion.Multiply(_rotation, Quaternion.CreateFromYawPitchRoll(0, deltaY * scale,0)));
_rotation.Normalize();
//m_rotationMat = Matrix.Multiply(Matrix.CreateFromYawPitchRoll(deltaX * scale, deltaY * scale, 0.0f),m_rotationMat);
if (_input.Keyboard.QPressedAndHeld) Translation += Vector3.Left;
if (_input.Keyboard.WPressedAndHeld) Translation += Vector3.Forward;
if (_input.Keyboard.EPressedAndHeld) Translation += Vector3.Right;
if (_input.Keyboard.SPressedAndHeld) Translation += Vector3.Backward;
if (_input.Keyboard.SpacePressedAndHeld) Translation += _input.Keyboard.ShiftPressed ? Vector3.Down : Vector3.Up;
Translation = Translation * TranslationSpeed * dt;
}
protected virtual void createViewMatrix()
{
Quaternion qPitch = Quaternion.CreateFromAxisAngle(Vector3.UnitX, MathHelper.ToRadians(_pitch));
Quaternion qYaw = Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.ToRadians(_yaw));
_orientation = qYaw*qPitch;
_orientation.Normalize();
_view = Matrix.CreateFromQuaternion(_orientation);
_view.Translation = _position;
Matrix.Invert(ref _view, out _view);
}
/// <summary>
/// QuaternionAngle returns the amount of rotation in the given quaternion, in radians.
/// </summary>
/// <param name="rotation">Input quaternion.</param>
/// <returns>Returns rotation angle in radians</returns>
public static float QuaternionAngle(Quaternion rotation)
{
rotation.Normalize();
float angle = 2.0f * (float)Math.Acos(rotation.W);
return angle;
}
/// <summary>
/// Performs the DES algorithm.
/// </summary>
protected void ComputeDESMatrix(ref Vector3 p, ref Quaternion q, out Matrix result)
{
// If translational alpha is 1, then no need to perform smoothing
if (transAlpha == 1)
transSt = p;
else
{
// Compute the translational smoothing
transSt = transAlpha * p + (1 - transAlpha) * (transPrevSt + transPrevBt);
transBt = transGamma * (transSt - transPrevSt) + (1 - transGamma) * transPrevBt;
transPrevSt = transSt;
transPrevBt = transBt;
}
// If rotational alpha is 1, then no need to perform smoothing
if (rotAlpha == 1)
rotSt = q;
else
{
rotSt = Quaternion.Slerp(rotPrevBt, q, rotAlpha);
rotBt = Quaternion.Slerp(rotPrevBt, rotSt, rotGamma);
rotSt.Normalize();
rotBt.Normalize();
rotPrevSt = rotSt;
rotPrevBt = rotBt;
}
Matrix.CreateFromQuaternion(ref rotSt, out tmpMat1);
Matrix.CreateTranslation(ref transSt, out tmpMat2);
Matrix.Multiply(ref tmpMat1, ref tmpMat2, out result);
}
public void Normalize()
{
Quaternion q = new Quaternion(1, 2, 3, 4);
Quaternion expected = new Quaternion(0.1825742f,0.3651484f,0.5477226f,0.7302967f);
Compare(expected, Quaternion.Normalize(q));
Quaternion result;
Quaternion.Normalize(ref q, out result);
Compare(expected, result);
q.Normalize();
Compare(expected, q);
}
/// <summary>
/// Call this method once per frame to update the internal state of the
/// Entity object.
/// </summary>
/// <param name="gameTime">The elapsed game time.</param>
public void Update(GameTime gameTime)
{
float elapsedTimeSec = (float)gameTime.ElapsedGameTime.TotalSeconds;
Vector3 velocityElapsed;
Vector3 eulerOrientElapsed;
Vector3 eulerRotateElapsed;
Vector3 oldPos;
Vector3 heading;
Quaternion temp;
velocityElapsed = velocity * elapsedTimeSec;
eulerOrientElapsed = eulerOrient * elapsedTimeSec;
eulerRotateElapsed = eulerRotate * elapsedTimeSec;
// Update the entity's position.
ExtractAxes();
oldPos = position;
position += right * velocityElapsed.X;
position += up * velocityElapsed.Y;
position += forward * velocityElapsed.Z;
heading = position - oldPos;
heading.Normalize();
// Update the entity's orientation.
temp = EulerToQuaternion(orientation, eulerOrientElapsed.Y,
eulerOrientElapsed.X, eulerOrientElapsed.Z);
// When moving backwards invert rotations to match direction of travel.
if (Vector3.Dot(heading, forward) < 0.0f)
temp = Quaternion.Inverse(temp);
orientation = Quaternion.Concatenate(orientation, temp);
orientation.Normalize();
// Update the entity's free rotation.
temp = EulerToQuaternion(rotation, eulerRotateElapsed.Y,
eulerRotateElapsed.X, eulerRotateElapsed.Z);
rotation = Quaternion.Concatenate(rotation, temp);
rotation.Normalize();
// Update the entity's world matrix.
temp = Quaternion.Concatenate(rotation, orientation);
temp.Normalize();
Matrix.CreateFromQuaternion(ref temp, out worldMatrix);
worldMatrix.M41 = position.X;
worldMatrix.M42 = position.Y;
worldMatrix.M43 = position.Z;
// Clear the cached Euler rotations and velocity for this frame.
velocity.X = velocity.Y = velocity.Z = 0.0f;
eulerOrient.X = eulerOrient.Y = eulerOrient.Z = 0.0f;
eulerRotate.X = eulerRotate.Y = eulerRotate.Z = 0.0f;
}
/// <summary>
/// Computes an arc rotation from two vectors
/// </summary>
/// <param name="origin">Origin</param>
/// <param name="destination">Destination</param>
/// <param name="fallback"></param>
/// <returns>Return a quaternion containing an arc rotation</returns>
public static Quaternion GetArcRotation(Vector3 origin, Vector3 destination, Vector3 fallback)
{
origin.Normalize();
destination.Normalize();
float dot;
Vector3.Dot(ref origin, ref destination, out dot);
if (dot >= 1.0f) return Quaternion.Identity;
Quaternion q;
if (dot < (1e-6f - 1.0f))
{
if (fallback != Vector3.Zero)
Quaternion.CreateFromAxisAngle(ref fallback, MathHelper.ToRadians(MathHelper.Pi), out q);
else
{
Vector3 axis = Vector3.Cross(Vector3.UnitZ, origin);
if (axis.IsZeroLength()) axis = Vector3.Cross(Vector3.UnitY, origin);
axis.Normalize();
Quaternion.CreateFromAxisAngle(ref axis, MathHelper.ToRadians(MathHelper.Pi), out q);
}
}
else
{
float s = (float)Math.Sqrt((1.0f + dot) * 2.0f);
float invS = 1.0f / s;
Vector3 cross;
Vector3.Cross(ref origin, ref destination, out cross);
#if XBOX || XBOX360
q = new Quaternion();
#endif
q.X = cross.X * invS;
q.Y = cross.Y * invS;
q.Z = cross.Z * invS;
q.W = s * 0.5f;
q.Normalize();
}
return q;
}
/// <summary>
/// Sets the world matrix
/// </summary>
/// <param name="time">The game time variable</param>
/// <param name="m">Rotation matrix</param>
public virtual void setWorldMatrix(float time, Matrix m)
{
#region "Rotation"
Quaternion pitch = Quaternion.CreateFromAxisAngle(m.Right, MathHelper.ToRadians(shipData.pitch) * time * pitchSpeed);
Quaternion roll = Quaternion.CreateFromAxisAngle(m.Backward, MathHelper.ToRadians(shipData.roll) * time * rollSpeed);
Quaternion yaw = Quaternion.CreateFromAxisAngle(m.Up, MathHelper.ToRadians(shipData.yaw) * time * yawSpeed);
rotate = yaw * pitch * roll * rotate;
rotate.Normalize();
#endregion
world = Matrix.CreateScale(shipData.scale) * Matrix.CreateFromQuaternion(rotate);
world.Translation = Position;
shipData.resetAngles();
}
private void Rotate(Quaternion qrot)
{
q = Quaternion.Multiply(qrot, q);
q.Normalize();
focus = Vector3.Transform(focusConst, q);
up = Vector3.Transform(upConst, q);
}
private void timer1_Tick(object sender, EventArgs e)
{
if (port2.IsOpen)
{
pictureBox1.BackColor = System.Drawing.Color.SpringGreen;
}
else
{
pictureBox1.BackColor = System.Drawing.Color.Red;
}
if (port.IsOpen)
{
pictureBox2.BackColor = System.Drawing.Color.SpringGreen;
}
else
{
pictureBox2.BackColor = System.Drawing.Color.Red;
}
int pom;
char[] ch = new char[8];
int[] serv = new int[6];
Vector3 eul;
Quaternion qu = new Quaternion();
qu.W = 1;
qu.X = 0;
qu.Y = -1;
qu.Z = 0;
qu.Normalize();
serv[0] = (int)(ad[0] * 0.5 + ad[1] * 0.5);
serv[1] = ad[2];
serv[2] = ad[4];
serv[3] = ad[6];
serv[4] = ad[8];
quat_out = Quaternion.Multiply(Quaternion.Conjugate(quat2), Quaternion.Multiply(quat1, qu)); //Quaternion.Multiply(Quaternion.Conjugate(quat2), quat1);
eul = FromQ2(quat_out);
if (eul.Z > 200)
{
serv[5] = (int)((eul.Z) / (double)0.990);
if (serv[5] < 0)
{ serv[5] = 0; }
if (serv[5] > 100)
{ serv[5] = 100; }
}
richTextBox5.Text = serv[0].ToString();
richTextBox6.Text = serv[1].ToString();
richTextBox7.Text = serv[2].ToString();
richTextBox8.Text = serv[3].ToString();
richTextBox9.Text = serv[4].ToString();
richTextBox10.Text = serv[5].ToString();
ch[0] = (char)101;
for (pom = 1; pom < 7; pom++)
{
ch[pom] = Convert.ToChar(serv[(pom - 1)]);
}
ch[7] = (char)101;
if (port2.IsOpen)
{
port2.Write(ch, 0, 8);
}
richTextBox1.Text = quat_out.W.ToString();
richTextBox2.Text = quat_out.X.ToString();
richTextBox3.Text = quat_out.Y.ToString();
richTextBox4.Text = quat_out.Z.ToString();
richTextBox11.Text = eul.Z.ToString();
richTextBox12.Text = eul.Y.ToString();
richTextBox13.Text = eul.X.ToString();
}
// helper method to convert quaternions to Euler angles
protected void QuaternionToEulerAngles(
ref Quaternion rotation, ref float pitch,
ref float yaw, ref float roll)
{
// following code assumes normalized quat
rotation.Normalize();
// temp variables to save some typing
float q0 = rotation.X;
float q1 = rotation.Y;
float q2 = rotation.Z;
float q3 = rotation.W;
// test for naughty cases (anomalies at north and south poles)
float check = q0 * q1 + q2 * q3;
if (Math.Abs(check) >= 0.499)
{
// special case to avoid unreliable data near poles
check /= Math.Abs(check); // determine sign (1 or -1)
// calculate angles for special case
pitch = check * (float)Math.PI / 2.0f;
yaw = check * 2.0f * (float)Math.Atan(q0 * q3);
roll = check * 0.0f;
}
else
{
// looks good; calculate angles for typical case
pitch = (float)Math.Asin(-2 * (q1 * q3 - q0 * q2));
yaw = (float)Math.Atan2(2 * (q1 * q2 + q0 * q3),
q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);
roll = (float)Math.Atan2(2 * (q2 * q3 + q0 * q1),
q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
}
// we're done; convert back to degrees
pitch = MathHelper.ToDegrees(pitch);
yaw = MathHelper.ToDegrees(yaw);
roll = MathHelper.ToDegrees(roll);
}
//Check for collision with a plane
private SpeedInfo applyCollisionPhysics(Plane plane, Vector3 speed)
{
SpeedInfo speedInfo;
speedInfo.speed = speed;
speedInfo.speedThisFrame = speed;
Vector3 speedInNormalDirection = Physics.dotProductCalculation(speed, plane.normal);
//Calculate the position of the end of the ball that is pointing toward the plane
Vector3 ballCollisionPoint = getPivotWithPlane(plane.normal);
CollisionInfo collisionInfo = applyCollisionPhysics(plane, speed, ballCollisionPoint, speedInNormalDirection);
if (collisionInfo != null)
{
float maxDistance = Math.Max(Math.Abs(collisionInfo.distanceTillCollision.X), Math.Abs(collisionInfo.distanceTillCollision.Y));
if (collisionInfo.distanceTillCollision.Length() == 0)
{
speedInfo = bounce(speed, speedInNormalDirection, plane.normal);
}
else if (maxDistance <= radius)
{
//Calculate the position of the end of the ball that is pointing toward the plane
Vector3 newPosition = ballCollisionPoint + speed; //The new position, ignoring collisions
CollisionInfo centerCollisionInfo = applyCollisionPhysics(plane, speed, position, speedInNormalDirection, true);
if (centerCollisionInfo != null)
{
Vector3 startingNormal = new Vector3(0, 1, 0);
Vector3 planeNormal = plane.normal;
planeNormal.Normalize();
Vector3 crossProduct = Vector3.Cross(startingNormal, planeNormal);
crossProduct.Normalize();
Vector3 distanceTillCollision = new Vector3(collisionInfo.distanceTillCollision.X, -1+(float) Math.Sqrt(collisionInfo.distanceTillCollision.X * collisionInfo.distanceTillCollision.X + collisionInfo.distanceTillCollision.Y * collisionInfo.distanceTillCollision.Y), collisionInfo.distanceTillCollision.Y);
if (!crossProduct.X.Equals(float.NaN) && !crossProduct.Y.Equals(float.NaN) && !crossProduct.Z.Equals(float.NaN))
{
Quaternion q = new Quaternion(crossProduct.X, crossProduct.Y, crossProduct.Z, 1 + Vector3.Dot(startingNormal, planeNormal));
q.Normalize();
distanceTillCollision = Vector3.Transform(distanceTillCollision, q);
}
Vector3 offSet = distanceTillCollision;
offSet = offSet * scale;
Vector3 recalculatedBallCollisionPoint = position + offSet;
Vector3 speedInCollisionDirection = Physics.dotProductCalculation(speed, offSet / scale);
if (Math.Sign(speedInCollisionDirection.X) != Math.Sign(offSet.X) || Math.Sign(speedInCollisionDirection.Y) != Math.Sign(offSet.Y) || Math.Sign(speedInCollisionDirection.Y) != Math.Sign(offSet.Y))
{
speedInCollisionDirection *= -1;
}
speedInfo = bounce(speed, speedInCollisionDirection, plane.normal);
}
}
}
return speedInfo;
}
public void UpdatePredictor(ref Vector3 p, ref Quaternion q)
{
if (initialized)
{
bool update = true;
// If translation threshold is set to larger than 0, then check whether
// the current transformation and previous transformation has larger translation
// difference than the threhold. If it is, then we ignore the currently passed
// transformation, and return the previously smoothed transformation.
if (transThreshold > 0)
{
float dist = Vector3.Distance(p, prevRawP);
if (dist > transThreshold)
update = false;
}
// If rotational threshold is set to larger than 0, then check whether
// the current transformation and previous transformation has larger angle
// difference than the threhold. If it is, then we ignore the currently passed
// transformation, and return the previously smoothed transformation.
if (rotThreshold > 0)
{
q.Normalize();
prevRawQ.Normalize();
float dotProduct = Quaternion.Dot(q, prevRawQ);
if (dotProduct > 1)
dotProduct = 1;
else if (dotProduct < -1)
dotProduct = -1;
float angleDiff = (float)Math.Acos(dotProduct);
if (angleDiff > rotThreshold)
update = false;
}
if (update)
{
transSp = transAlpha * p + (1 - transAlpha) * transSp;
transSp2 = transAlpha * transSp + (1 - transAlpha) * transSp2;
prevRawP = p;
prevRawQ = q;
restartCount = 0;
}
else
{
restartCount++;
if (restartCount > RESTART_THRESHOLD)
{
prevRawP = p;
prevRawQ = q;
restartCount = 0;
}
}
}
else
{
transSp = transSp2 = p;
rotSq = rotSq2 = q;
prevRawP = p;
prevRawQ = q;
initialized = true;
}
}
/// <summary>
/// Gets a filtered matrix using double exponential smoothing algorithm.
/// </summary>
/// <param name="p">The original position</param>
/// <param name="q">The original rotation</param>
/// <param name="result">A smoothed matrix</param>
public virtual void FilterMatrix(ref Vector3 p, ref Quaternion q, out Matrix result)
{
if (initialized)
{
bool compute = true;
// If translation threshold is set to larger than 0, then check whether
// the current transformation and previous transformation has larger translation
// difference than the threhold. If it is, then we ignore the currently passed
// transformation, and return the previously smoothed transformation.
if (transThreshold > 0)
{
float dist = 0;
Vector3.Distance(ref p, ref prevRawP, out dist);
if (dist > transThreshold)
compute = false;
}
// If rotational threshold is set to larger than 0, then check whether
// the current transformation and previous transformation has larger angle
// difference than the threhold. If it is, then we ignore the currently passed
// transformation, and return the previously smoothed transformation.
if (rotThreshold > 0)
{
q.Normalize();
prevRawQ.Normalize();
float dotProduct = 0;
Quaternion.Dot(ref q, ref prevRawQ, out dotProduct);
if (dotProduct > 1)
dotProduct = 1;
else if (dotProduct < -1)
dotProduct = -1;
float angleDiff = (float)Math.Acos(dotProduct);
if (angleDiff > rotThreshold)
compute = false;
}
if (compute)
{
ComputeDESMatrix(ref p, ref q, out prevComputedMat);
prevRawP = p;
prevRawQ = q;
restartCount = 0;
}
else
{
restartCount++;
if (restartCount > RESTART_THRESHOLD)
{
prevRawP = p;
prevRawQ = q;
restartCount = 0;
}
}
result = prevComputedMat;
}
else
{
if (tmpTrans.Equals(Vector3.Zero))
{
tmpTrans = p;
tmpRot = q;
}
else
{
transPrevSt = p;
transPrevBt = p - tmpTrans;
rotPrevSt = q;
rotPrevBt = q;
prevRawP = p;
prevRawQ = q;
initialized = true;
}
Matrix.CreateFromQuaternion(ref q, out tmpMat1);
Matrix.CreateTranslation(ref p, out tmpMat2);
// return unfiltered matrix since can not smooth yet
Matrix.Multiply(ref tmpMat1, ref tmpMat2, out result);
}
}
public override void Update(GameTime gt)
{
gameTime = gt.TotalGameTime.Ticks;
Position += moment;
Orientation = Quaternion.Concatenate(Orientation, Quaternion.CreateFromAxisAngle(model.Root.Transform.Up, (float)(rotation.Y * turnConst)));
Orientation = Quaternion.Concatenate(Orientation, Quaternion.CreateFromAxisAngle(model.Root.Transform.Right, (float)(rotation.X * turnConst)));
Orientation = Quaternion.Concatenate(Orientation, Quaternion.CreateFromAxisAngle(model.Root.Transform.Forward, (float)(rotation.Z * turnConst)));
Orientation.Normalize();
Speed = Vector3.Transform(moment, Orientation);
model.Root.Transform = Matrix.CreateFromQuaternion(Orientation) * Matrix.CreateTranslation(Position);
model.Bones["Radar"].Transform *= Matrix.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(1.5f));
model.CopyAbsoluteBoneTransformsTo(transforms);
Matrix slot = transforms[model.Bones["Slot"].Index];
Vector3 tt = Vector3.Transform(Vector3.Zero, Matrix.Invert(slot));
tt.Normalize();
Matrix turret = model.Bones["Turret"].Transform;
Vector3 front = turret.Forward;
if(Vector3.Dot(front, tt) > -0.99) {
front = Vector3.Lerp(front, tt, 0.1f);
} else {
// Special case for if we are turning exactly 180 degrees.
front = Vector3.Lerp(front, turret.Right, 0.1f);
}
Vector3 right = Vector3.Cross(front, Vector3.Up);
Vector3 up = Vector3.Cross(right, front);
front.Normalize();
right.Normalize();
up.Normalize();
if(Vector3.Dot(front, Vector3.Up) > -0.1f) {
model.Bones["Turret"].Transform = Matrix.CreateWorld(Vector3.Zero, front, up);
model.CopyAbsoluteBoneTransformsTo(transforms);
}
}
public override void Update(GameTime gameTime)
{
float timeDelta = (float)gameTime.ElapsedGameTime.TotalSeconds;
KeyboardState state = Keyboard.GetState();
if (state.IsKeyDown(Keys.Space))
{
addForce(look * 10.0f);
}
// Yaw
if (state.IsKeyDown(Keys.J))
{
addTorque(this.up * 10.0f);
}
if (state.IsKeyDown(Keys.L))
{
addTorque(this.up * -10.0f);
}
// End of Yaw
//Pitch
if (state.IsKeyDown(Keys.I))
{
addTorque(this.right * 10.0f);
}
if (state.IsKeyDown(Keys.K))
{
addTorque(this.right * -10.0f);
}
// End of Pitch
if (state.IsKeyDown(Keys.Y))
{
addTorque(this.look * 10.0f);
}
if (state.IsKeyDown(Keys.H))
{
addTorque(this.look * -10.0f);
}
// Do the Newtonian integration
acceleration = force / mass;
velocity += acceleration * timeDelta;
pos += velocity * timeDelta;
force = Vector3.Zero;
if (velocity.Length() > 0.0001f)
{
look = Vector3.Normalize(velocity);
right = Vector3.Cross(look, up);
velocity *= 0.99f;
}
// Do the Hamiltonian integration
angularAcceleration = Vector3.Transform(torque, Matrix.Invert(inertialTensor));
angularVelocity = angularVelocity + angularAcceleration * timeDelta;
Quaternion w = new Quaternion(angularVelocity.X, angularVelocity.Y, angularVelocity.Z, 0);
quaternion += ((w * (timeDelta / 2.0f)) * quaternion);
quaternion.Normalize();
torque = Vector3.Zero;
// Recalculate the Look, up and right
w = new Quaternion(Vector3.Forward, 0);
w = quaternion * w * Quaternion.Inverse(quaternion);
look.X = w.X;
look.Y = w.Y;
look.Z = w.Z;
w = new Quaternion(Vector3.Up, 0);
w = quaternion * w * Quaternion.Inverse(quaternion);
up.X = w.X;
up.Y = w.Y;
up.Z = w.Z;
w = new Quaternion(Vector3.Right, 0);
w = quaternion * w * Quaternion.Inverse(quaternion);
right.X = w.X;
right.Y = w.Y;
right.Z = w.Z;
}
/// <summary>
/// Advance the simulator by one timestep
/// </summary>
/// <param name="time"></param>
public void update(GameTime time)
{
// restest counters and stop watches
mNumCollisionTests = 0;
mTimePhysics.Reset();
mTimeCollisions.Reset();
mTimePhysics.Start();
mCollidingFaces.Clear();
// time delta
float dt = time.ElapsedGameTime.Milliseconds * timeScale / numSubSteps;
// for each substep
for (int step = 0; step < numSubSteps; step++)
{
// iterate through all spheres
LinkedListNode<SphereProperties> curSimulatedSphere = mSimulatedSpheres.First;
LinkedListNode<SphereProperties> nextSimulatedSphere = mSimulatedSpheresNextStep.First;
while (curSimulatedSphere != null)
{
// copy current sphere properties
SphereProperties sphere = curSimulatedSphere.Value;
// compute forces
Vector3 forces = -mLinearFriction * curSimulatedSphere.Value.velocity;
forces += mGravity;
// compute acceleration
Vector3 acceleration = forces / mMass;
// integrate (keep it simple: just use euler)
sphere.velocity += acceleration * dt;
sphere.position += sphere.velocity * dt;
// compute new angular momentum (damping)
sphere.angularMomentum *= mAngularDamping;
// integrate angular momentum
Matrix rot = Matrix.CreateFromQuaternion(curSimulatedSphere.Value.rotation);
sphere.invWorldInertia = rot * sphere.invBodyInertia * Matrix.Transpose(rot);
// compute angular velocity
Vector3 omega = Vector3.Transform(sphere.angularMomentum, sphere.invWorldInertia);
// integrate rotational part into quaternion
Quaternion quatRotDot = new Quaternion(omega, 0.0f) * sphere.rotation;
quatRotDot *= -0.5f;
quatRotDot *= dt;
quatRotDot += sphere.rotation;
quatRotDot.Normalize();
sphere.rotation = quatRotDot;
// check for arena collisions and resolve them
handleArenaCollisions(ref sphere);
// copy new sphere properties to updated buffer
nextSimulatedSphere.Value = sphere;
// remove spheres below arena
LinkedListNode<SphereProperties> oldNode = curSimulatedSphere;
LinkedListNode<SphereProperties> oldNode2 = nextSimulatedSphere;
curSimulatedSphere = curSimulatedSphere.Next;
nextSimulatedSphere = nextSimulatedSphere.Next;
if (sphere.position.Y < -200)
{
mSimulatedSpheres.Remove(oldNode);
mSimulatedSpheresNextStep.Remove(oldNode2);
}
}
// swap lists (update all spheres at once)
LinkedList<SphereProperties> tmp = mSimulatedSpheres;
mSimulatedSpheres = mSimulatedSpheresNextStep;
mSimulatedSpheresNextStep = tmp;
// compute and resolve sphere-sphere collisions
handleSphereSphereCollisions();
}
// stop stopwatch
mTimePhysics.Stop();
}
/// <summary>
/// QuaternionAngle returns the axis and amount of rotation in the given quaternion, in radians.
/// </summary>
/// <param name="rotation">The input quaternion.</param>
/// <returns>Returns the axis angle of rotation in a Vector4.</returns>
public static Microsoft.Xna.Framework.Vector4 QuaternionToAxisAngle(Quaternion rotation)
{
rotation.Normalize();
float angle = 2.0f * (float)Math.Acos(rotation.W);
float s = (float)Math.Sqrt(1.0f - (rotation.W * rotation.W));
// If the angle is very small, the direction is not important - set a default here
Vector3 axis = new Vector3(rotation.X, rotation.Y, rotation.Z);
// perform calculation if proper angle
if (s >= 0.001f)
{
float oneOverS = 1.0f / s;
axis.X = rotation.X * oneOverS; // normalize axis
axis.Y = rotation.Y * oneOverS;
axis.Z = rotation.Z * oneOverS;
}
axis.Normalize();
return new Microsoft.Xna.Framework.Vector4(axis, angle);
}
public void AutoPilot(float delta)
{
// timeSinceLastTurnCheck += delta;
#region RandomizeThurst
//Get a random float
float randomThrust = (float)random.NextDouble();
// If the float is less than .1, make it .1 so it doesn't just stop
if (randomThrust < .1f) randomThrust = .1f;
//Assign the randomThrust to the thrust
thrust = randomThrust;
#endregion
// #region UpdateWings
// // The furthest out the wings can flap
// const float MaxAngle = 2f;
// // The closest in the wings can flap
// const float MinAngle = 0f;
// // The total distance (radians) that the wings are moving
// const float DeploymentAngle = MaxAngle - MinAngle;
// // The amount of time in which it should move the total distance
// const float DeploymentTime = .2f;
// if // The wings should be opening
// (wingsGoingDown && wingAngle < MaxAngle)
// {
// wingAngle += (float)(DeploymentAngle * delta / DeploymentTime);
// }
// else if // The wings should be closing
// (!wingsGoingDown && wingAngle > -MinAngle)
// {
// wingAngle -= (float)(DeploymentAngle * delta / DeploymentTime);
// }
// else // The wings have hit the end; they need to go the other way
// {
// wingsGoingDown = !wingsGoingDown;
// }
// #endregion
#region RandomizeTurnRate
// 10% chance every 1/2 second for the sign to be flipped (this may vary system to system)
timeSinceLastTurnCheck += delta;
if (timeSinceLastTurnCheck >= .5f)
{
timeSinceLastTurnCheck = 0;
if (random.Next(5) == 1)
{
signOfTurnRate = -signOfTurnRate;
}
}
// Get a random float to be the turnRate
float randomTurnRate = (float)random.NextDouble();
// Make the turnRate the randomTurnRate but the same sign as it should be
turnRate = randomTurnRate * signOfTurnRate;
#endregion
#region UpdateOrientation
// Get the amout to which the locust should be turning
float turnAngle = turnRate * MaxTurnRate * delta;
// Create a new orientation vector based where the locust is facing
orientation *= Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), turnAngle);// *
// Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), pitchRate * MaxPitchRate * delta) *
// Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), -turnAngle);
// Normalize the orientation to prevent round-off error
orientation.Normalize();
#endregion
#region UpdatePosition
// Get the current acceleration of the locust
float acceleration = thrust * MaxThrust - Drag * speed;
// Update the speed of the locust
speed += acceleration * delta;
// Depending on where the locust is facing, update the transform
Matrix transform = Matrix.CreateFromQuaternion(orientation);
// Get the thrust to which the locust is facing
Vector3 directedThrust = Vector3.TransformNormal(new Vector3(0, 0, 1), transform);
// Update the position based on the thrust, speed, and time since last update
Vector3 newPosition = position + directedThrust * speed * delta;
#region UpdatePosition
//Vector3 newPosition = position + delta * new Vector3(100, 0, 0);
string region = collisionDetector.TestRegion(newPosition);
//If region doesn't exist, don't allow you to go in it
if (region.Equals(String.Empty))
{
}//NTOE IT GOES THRU DOORS
else//Otherwise, it's a valid location
{
//location is in the bounds of the map
position = newPosition;
}
//position.Y = 0;
#endregion
#endregion
}
