public void ApplyChanges()
{
var position = rigidBody.Position;
var rotation = JQuaternion.CreateFromMatrix(rigidBody.Orientation);
gameObject.transform.position = new Vector3(position.X, position.Y, position.Z);
gameObject.transform.rotation = new Quaternion(rotation.X, rotation.Y, rotation.Z, rotation.W);
}
private ScalableTransform GetTransform()
{
var current = new Transform(JQuaternion.CreateFromMatrix(Body.Orientation).ToQuaternion(),
Body.Position.ToVector3());
var t = current * baseTransform;
return(new ScalableTransform(t.Rotation, t.Translation, storedScale));
}
public static Quaternion ConvertToQuaternion(this JQuaternion jq, Quaternion q)
{
q.w = jq.W;
q.x = jq.X;
q.y = jq.Y;
q.z = jq.Z;
return(q);
}
internal static Quaternion ToArenaData(JQuaternion v)
{
return(new Quaternion
{
X = v.X,
Y = v.Y,
Z = v.Z,
W = v.W
});
}
public static JQuaternion Conjugate(JQuaternion value)
{
JQuaternion quaternion;
quaternion.X = -value.X;
quaternion.Y = -value.Y;
quaternion.Z = -value.Z;
quaternion.W = value.W;
return(quaternion);
}
public override void Update(float dt)
{
world.Step(dt, false);
foreach (JitterBody body in Entity.GetAll <JitterBody>())
{
Transform t = Entity.Get <Transform>(body.Id);
t.Position.Value = Convert(body.Rigidbody.Position);
JQuaternion rot = JQuaternion.CreateFromMatrix(body.Rigidbody.Orientation);
t.Rotation.Value = Convert(rot);
}
}
private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
JVector temp;
JVector.Multiply(ref body.linearVelocity, timestep, out temp);
JVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
JVector axis;
double angle = body.angularVelocity.Length();
if (angle < 0.001f)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
JVector.Multiply(ref body.angularVelocity, (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
JVector.Multiply(ref body.angularVelocity, ((double)Math.Sin(0.5f * angle * timestep) / angle), out axis);
}
JQuaternion dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (double)Math.Cos(angle * timestep * 0.5f));
JQuaternion ornA; JQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
JQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); JMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
if ((body.Damping & RigidBody.DampingType.Linear) != 0)
{
JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
}
if ((body.Damping & RigidBody.DampingType.Angular) != 0)
{
JVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);
}
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
public void RestoreState(BinaryReader reader)
{
Body.Position = new JVector(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
var q = new JQuaternion(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
Body.Orientation = JMatrix.CreateFromQuaternion(q);
if (Body.IsStatic)
{
return;
}
Body.LinearVelocity = new JVector(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
Body.AngularVelocity = new JVector(reader.ReadSingle(), reader.ReadSingle(), reader.ReadSingle());
}
public void SaveState(BinaryWriter writer)
{
writer.Write(Body.Position.X); writer.Write(Body.Position.Y); writer.Write(Body.Position.Z);
var q = JQuaternion.CreateFromMatrix(Body.Orientation);
writer.Write(q.X); writer.Write(q.Y); writer.Write(q.Z); writer.Write(q.W);
if (Body.IsStatic)
{
return;
}
writer.Write(Body.LinearVelocity.X); writer.Write(Body.LinearVelocity.Y); writer.Write(Body.LinearVelocity.Z);
writer.Write(Body.AngularVelocity.X); writer.Write(Body.AngularVelocity.Y); writer.Write(Body.AngularVelocity.Z);
}
protected override void OnAdded(ComponentStateEventArgs registrationArgs)
{
base.OnAdded(registrationArgs);
Body.Tag = Record.Name;
var offset = Body.Position;
var rot = JMatrix.CreateFromQuaternion(transform.Rotation.ToQuaternion());
JVector.Transform(ref offset, ref rot, out offset);
Offset = offset.ToVector3();
Body.Orientation = Body.Orientation * rot;
Body.Position = offset + transform.Translation.ToJVector();
baseTransform = new Transform(JQuaternion.CreateFromMatrix(Body.Orientation).ToQuaternion(),
Body.Position.ToVector3()).Invert() * new Transform(transform.Rotation, transform.Translation);
storedScale = transform.Scale;
transform.Bind(GetTransform, SetTransform);
}
/// <summary>
/// Functionality which is applied before the physics is updated
/// </summary>
/// <param name="timestep"></param>
public override void PreStep(float timestep)
{
if (IsAnimated)
{
_oldRotation = Conversion.ToXnaQuaternion(JQuaternion.CreateFromMatrix(Orientation)).ToEuler().Y;
_newRotation = GameObject.transform.eulerAngles.Y;
// First calculate the correct orientation/rotation and then adjust the position with respect to the rotated COM
Orientation = JMatrix.CreateFromQuaternion(Conversion.ToJitterQuaternion(GameObject.transform.rotation));
Position = Conversion.ToJitterVector(GameObject.transform.position) + JVector.Transform(CenterOfMass, Orientation);
}
else
{
_oldRotation = GameObject.transform.eulerAngles.Y;
}
}
private void IntegrateCallback(object obj)
{
var body = obj as RigidBody;
JVector temp;
JVector.Multiply(ref body.linearVelocity, timestep, out temp);
JVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
JVector axis;
float angle = body.angularVelocity.Length();
if (angle < 0.001f)
{
JVector.Multiply(ref body.angularVelocity,
(0.5f*timestep - (timestep*timestep*timestep)*(0.020833333333f)*angle*angle),
out axis);
}
else
{
JVector.Multiply(ref body.angularVelocity, ((float) Math.Sin(0.5f*angle*timestep)/angle), out axis);
}
var dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (float) Math.Cos(angle*timestep*0.5f));
JQuaternion ornA;
JQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
JQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize();
JMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
if ((body.Damping & RigidBody.DampingType.Linear) != 0)
JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
if ((body.Damping & RigidBody.DampingType.Angular) != 0)
JVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
body.SweptExpandBoundingBox(timestep);
}
/// <summary>
/// Functionality which is applied after the physics is updated
/// </summary>
/// <param name="timestep"></param>
public override void PostStep(float timestep)
{
if (!IsStatic && !IsAnimated)
{
GameObject.transform.position = Conversion.ToXnaVector(Position - JVector.Transform(CenterOfMass, Orientation));
GameObject.transform.rotation = Conversion.ToXnaQuaternion(JQuaternion.CreateFromMatrix(Orientation));
_newRotation = GameObject.transform.eulerAngles.Y;
//float test = (float)Math.Acos(Vector3.Dot(GameObject.transform.Forward, Vector3.Forward));
//Debug.Log(test + " == " + _newRotation);
}
if (SyncedObjects.Count > 0)
{
Vector3 projectedToGround = GameObject.transform.position;
projectedToGround.Y = 0;
float rotation = GameObject.transform.eulerAngles.Y;
foreach (Follower follower in SyncedObjects)
{
switch (follower.Type)
{
case Follower.FollowingType.OnFloor:
float offset = MathHelper.Clamp(GameObject.transform.position.Y, 1, 10);
follower.GameObject.transform.rotation = Quaternion.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(rotation));
follower.GameObject.transform.position = projectedToGround + offset * follower.Offset;
break;
case Follower.FollowingType.Orientated:
var tmp = Quaternion.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(rotation));
follower.GameObject.transform.rotation = tmp * follower.Orientation;
follower.GameObject.transform.position = projectedToGround + Vector3.Transform(follower.Offset, tmp);
break;
}
}
}
base.PostStep(timestep);
}
/// <summary>
/// Multiply two quaternions.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The product of both quaternions.</param>
public static void Multiply(ref JQuaternion quaternion1, ref JQuaternion quaternion2, out JQuaternion result)
{
Fix64 x = quaternion1.X;
Fix64 y = quaternion1.Y;
Fix64 z = quaternion1.Z;
Fix64 w = quaternion1.W;
Fix64 num4 = quaternion2.X;
Fix64 num3 = quaternion2.Y;
Fix64 num2 = quaternion2.Z;
Fix64 num = quaternion2.W;
Fix64 num12 = (y * num2) - (z * num3);
Fix64 num11 = (z * num4) - (x * num2);
Fix64 num10 = (x * num3) - (y * num4);
Fix64 num9 = ((x * num4) + (y * num3)) + (z * num2);
result.X = ((x * num) + (num4 * w)) + num12;
result.Y = ((y * num) + (num3 * w)) + num11;
result.Z = ((z * num) + (num2 * w)) + num10;
result.W = (w * num) - num9;
}
/// <summary>
/// Creates a quaternion from a matrix.
/// </summary>
/// <param name="matrix">A matrix representing an orientation.</param>
/// <param name="result">JQuaternion representing an orientation.</param>
public static void CreateFromMatrix(ref JMatrix matrix, out JQuaternion result)
{
Fix64 num8 = (matrix.M11 + matrix.M22) + matrix.M33;
if (num8 > 0f)
{
Fix64 num = (Fix64)Fix64.Sqrt((num8 + 1f));
result.W = num * 0.5f;
num = 0.5f / num;
result.X = (matrix.M23 - matrix.M32) * num;
result.Y = (matrix.M31 - matrix.M13) * num;
result.Z = (matrix.M12 - matrix.M21) * num;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
Fix64 num7 = (Fix64)Fix64.Sqrt((((1f + matrix.M11) - matrix.M22) - matrix.M33));
Fix64 num4 = 0.5f / num7;
result.X = 0.5f * num7;
result.Y = (matrix.M12 + matrix.M21) * num4;
result.Z = (matrix.M13 + matrix.M31) * num4;
result.W = (matrix.M23 - matrix.M32) * num4;
}
else if (matrix.M22 > matrix.M33)
{
Fix64 num6 = (Fix64)Fix64.Sqrt((((1f + matrix.M22) - matrix.M11) - matrix.M33));
Fix64 num3 = 0.5f / num6;
result.X = (matrix.M21 + matrix.M12) * num3;
result.Y = 0.5f * num6;
result.Z = (matrix.M32 + matrix.M23) * num3;
result.W = (matrix.M31 - matrix.M13) * num3;
}
else
{
Fix64 num5 = (Fix64)Fix64.Sqrt((((1f + matrix.M33) - matrix.M11) - matrix.M22));
Fix64 num2 = 0.5f / num5;
result.X = (matrix.M31 + matrix.M13) * num2;
result.Y = (matrix.M32 + matrix.M23) * num2;
result.Z = 0.5f * num5;
result.W = (matrix.M12 - matrix.M21) * num2;
}
}
public static Quaternion ToQuaternion(this JQuaternion rot)
{
return(new Quaternion(rot.X, rot.Y, rot.Z, rot.W));
}
public static Quaternion TK(this JQuaternion quat)
{
return(new Quaternion(quat.X, quat.Y, quat.Z, quat.W));
}
private void Update(float totalSecondsElapsed)
{
var originalRotation = new Dictionary <int, Quaternion>();
var originalPosition = new Dictionary <int, Vector3>();
_stopwatch.Start();
_physicsMetrics.StaticImmovableObjects = 0;
_physicsMetrics.PhysicsObjects = 0;
foreach (var kv in _rigidBodyMappings)
{
if (kv.StaticAndImmovable)
{
_physicsMetrics.StaticImmovableObjects++;
}
else
{
_physicsMetrics.PhysicsObjects++;
}
if (kv.StaticAndImmovable && kv.PerformedInitialSync)
{
continue;
}
var rigidBody = kv.RigidBody;
var hasTransform = kv.HasTransform;
// Put the lookup in the transform cache.
_transformCache[rigidBody.GetHashCode()] = new WeakReference <IHasTransform>(hasTransform);
// Sync game world to physics system.
var rot = hasTransform.FinalTransform.AbsoluteRotation;
var pos = hasTransform.FinalTransform.AbsolutePosition;
originalRotation[rigidBody.GetHashCode()] = rot;
originalPosition[rigidBody.GetHashCode()] = pos;
// If the position of the entity differs from what we expect, then the user
// probably explicitly set it and we need to sync the rigid body.
if (_lastFramePosition.ContainsKey(rigidBody.GetHashCode()))
{
var lastPosition = _lastFramePosition[rigidBody.GetHashCode()];
if ((lastPosition - hasTransform.Transform.LocalPosition).LengthSquared() > 0.0001f)
{
rigidBody.Position = pos.ToJitterVector();
}
}
else
{
rigidBody.Position = pos.ToJitterVector();
}
// If the rotation of the entity differs from what we expect, then the user
// probably explicitly set it and we need to sync the rigid body.
if (_lastFrameRotation.ContainsKey(rigidBody.GetHashCode()))
{
var lastRotation = _lastFrameRotation[rigidBody.GetHashCode()];
var a = Quaternion.Normalize(lastRotation);
var b = Quaternion.Normalize(hasTransform.Transform.LocalRotation);
var closeness = 1 - (a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W);
if (closeness > 0.0001f)
{
rigidBody.Orientation = JMatrix.CreateFromQuaternion(rot.ToJitterQuaternion());
}
}
else
{
rigidBody.Orientation = JMatrix.CreateFromQuaternion(rot.ToJitterQuaternion());
}
}
_lastFramePosition.Clear();
_lastFrameRotation.Clear();
_stopwatch.Stop();
_physicsMetrics.SyncToPhysicsTime = _stopwatch.Elapsed.TotalMilliseconds;
_stopwatch.Restart();
_physicsWorld.Step(totalSecondsElapsed, true);
_stopwatch.Stop();
_physicsMetrics.PhysicsStepTime = _stopwatch.Elapsed.TotalMilliseconds;
_stopwatch.Restart();
foreach (var kv in _rigidBodyMappings)
{
if (kv.StaticAndImmovable && kv.PerformedInitialSync)
{
continue;
}
var rigidBody = kv.RigidBody;
var hasMatrix = kv.HasTransform;
// Calculate the changes that the physics system made in world space.
var oldWorldRot = Quaternion.Normalize(originalRotation[rigidBody.GetHashCode()]);
var oldWorldPos = originalPosition[rigidBody.GetHashCode()];
var newWorldRot = Quaternion.Normalize(JQuaternion.CreateFromMatrix(rigidBody.Orientation).ToXNAQuaternion());
var newWorldPos = rigidBody.Position.ToXNAVector();
// Determine the localised differences in position.
var localPos = newWorldPos - oldWorldPos;
// Update the local components of the transform.
hasMatrix.Transform.LocalPosition += localPos;
hasMatrix.Transform.LocalRotation *= Quaternion.Inverse(oldWorldRot) * newWorldRot;
// Save the current rotation / position for the next frame.
_lastFramePosition[rigidBody.GetHashCode()] = hasMatrix.Transform.LocalPosition;
_lastFrameRotation[rigidBody.GetHashCode()] = hasMatrix.Transform.LocalRotation;
if (kv.StaticAndImmovable && !kv.PerformedInitialSync)
{
kv.PerformedInitialSync = true;
}
}
_stopwatch.Stop();
_physicsMetrics.SyncFromPhysicsTime = _stopwatch.Elapsed.TotalMilliseconds;
_stopwatch.Reset();
}
private static JMatrix to_j(Quaternion r)
{
var q = new JQuaternion(r.X, r.Y, r.Z, r.W);
return(JMatrix.CreateFromQuaternion(q));
}
public static quat ToQuat(this JMatrix m)
{
var q = JQuaternion.CreateFromMatrix(m);
return(new quat(q.X, q.Y, q.Z, q.W));
}
private void Update(float totalSecondsElapsed)
{
var originalRotation = new Dictionary <int, Quaternion>();
var originalPosition = new Dictionary <int, Vector3>();
_stopwatch.Start();
_physicsMetrics.StaticImmovableObjects = 0;
_physicsMetrics.PhysicsObjects = 0;
var gcRigidBodyMappings = new List <RigidBodyMapping>();
foreach (var kv in _rigidBodyMappings)
{
if (kv.StaticAndImmovable)
{
_physicsMetrics.StaticImmovableObjects++;
}
else
{
_physicsMetrics.PhysicsObjects++;
}
if (kv.StaticAndImmovable && kv.PerformedInitialSync)
{
continue;
}
var rigidBody = kv.RigidBody;
IHasTransform hasTransform;
if (!kv.HasTransform.TryGetTarget(out hasTransform))
{
// The transform has been garbage collected. We need to mark the rigid body mapping
// as pending deletion and skip it for now.
gcRigidBodyMappings.Add(kv);
continue;
}
// Sync game world to physics system.
var rot = hasTransform.FinalTransform.AbsoluteRotation;
var pos = hasTransform.FinalTransform.AbsolutePosition;
originalRotation[rigidBody.GetHashCode()] = rot;
originalPosition[rigidBody.GetHashCode()] = pos;
// If the position of the entity differs from what we expect, then the user
// probably explicitly set it and we need to sync the rigid body.
if (_lastFramePosition.ContainsKey(rigidBody.GetHashCode()))
{
var lastPosition = _lastFramePosition[rigidBody.GetHashCode()];
if ((lastPosition - hasTransform.Transform.LocalPosition).LengthSquared() > 0.0001f)
{
rigidBody.Position = pos.ToJitterVector();
}
}
else
{
rigidBody.Position = pos.ToJitterVector();
}
// If the rotation of the entity differs from what we expect, then the user
// probably explicitly set it and we need to sync the rigid body.
if (_lastFrameRotation.ContainsKey(rigidBody.GetHashCode()))
{
var lastRotation = _lastFrameRotation[rigidBody.GetHashCode()];
var a = Quaternion.Normalize(lastRotation);
var b = Quaternion.Normalize(hasTransform.Transform.LocalRotation);
var closeness = 1 - (a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W);
if (closeness > 0.0001f)
{
rigidBody.Orientation = JMatrix.CreateFromQuaternion(rot.ToJitterQuaternion());
}
}
else
{
rigidBody.Orientation = JMatrix.CreateFromQuaternion(rot.ToJitterQuaternion());
}
}
if (gcRigidBodyMappings.Count > 0)
{
foreach (var kv in gcRigidBodyMappings)
{
// Remove the rigid body whose transform has been garbage collected.
_physicsWorld.RemoveBody(kv.RigidBody);
_rigidBodyMappings.Remove(kv);
}
}
_lastFramePosition.Clear();
_lastFrameRotation.Clear();
_stopwatch.Stop();
_physicsMetrics.SyncToPhysicsTime = _stopwatch.Elapsed.TotalMilliseconds;
_stopwatch.Restart();
_physicsWorld.Step(totalSecondsElapsed, true);
_stopwatch.Stop();
_physicsMetrics.PhysicsStepTime = _stopwatch.Elapsed.TotalMilliseconds;
_stopwatch.Restart();
foreach (var kv in _rigidBodyMappings)
{
if (kv.StaticAndImmovable && kv.PerformedInitialSync)
{
continue;
}
var rigidBody = kv.RigidBody;
IHasTransform hasTransform;
if (!kv.HasTransform.TryGetTarget(out hasTransform))
{
// The transform has been garbage collected. Next step we'll detect it's been garbage
// collected and deal with it then.
continue;
}
// Calculate the changes that the physics system made in world space.
var oldWorldRot = Quaternion.Normalize(originalRotation[rigidBody.GetHashCode()]);
var oldWorldPos = originalPosition[rigidBody.GetHashCode()];
var newWorldRot = Quaternion.Normalize(JQuaternion.CreateFromMatrix(rigidBody.Orientation).ToXNAQuaternion());
var newWorldPos = rigidBody.Position.ToXNAVector();
// Determine the localised differences in position.
var localPos = newWorldPos - oldWorldPos;
// Update the local components of the transform.
hasTransform.Transform.LocalPosition += localPos;
hasTransform.Transform.LocalRotation *= Quaternion.Inverse(oldWorldRot) * newWorldRot;
// Save the current rotation / position for the next frame.
_lastFramePosition[rigidBody.GetHashCode()] = hasTransform.Transform.LocalPosition;
_lastFrameRotation[rigidBody.GetHashCode()] = hasTransform.Transform.LocalRotation;
if (kv.StaticAndImmovable && !kv.PerformedInitialSync)
{
kv.PerformedInitialSync = true;
}
}
_stopwatch.Stop();
_physicsMetrics.SyncFromPhysicsTime = _stopwatch.Elapsed.TotalMilliseconds;
_stopwatch.Reset();
}
/// <summary>
/// Convert a Jitter-quaternion to a XNA-quaternion
/// </summary>
/// <param name="quaternion">Jitter-quaternion</param>
/// <returns>XNA-quaternion</returns>
public static Quaternion ToXnaQuaternion(JQuaternion quaternion)
{
return(new Quaternion(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W));
}
public void FromBitStream(ReadOnlyBitStream packetStream)
{
float positionX = packetStream.ReadSingle();
float positionY = packetStream.ReadSingle();
float positionZ = packetStream.ReadSingle();
Position = new JVector(positionX, positionY, positionZ);
float rotationX = packetStream.ReadSingle();
float rotationY = packetStream.ReadSingle();
float rotationZ = packetStream.ReadSingle();
float rotationW = packetStream.ReadSingle();
Rotation = new JQuaternion(rotationX, rotationY, rotationZ, rotationW);
IsSupported = packetStream.ReadBit();
IsOnRail = packetStream.ReadBit();
bool flag = packetStream.ReadBit();
if (flag)
{
float linearVelocityX = packetStream.ReadSingle();
float linearVelocityY = packetStream.ReadSingle();
float linearVelocityZ = packetStream.ReadSingle();
LinearVelocity = new JVector(linearVelocityX, linearVelocityY, linearVelocityZ);
}
flag = packetStream.ReadBit();
if (flag)
{
float angularVelocityX = packetStream.ReadSingle();
float angularVelocityY = packetStream.ReadSingle();
float angularVelocityZ = packetStream.ReadSingle();
AngularVelocity = new JVector(angularVelocityX, angularVelocityY, angularVelocityZ);
}
flag = packetStream.ReadBit();
if (flag)
{
LocalSpaceObjectId = packetStream.ReadInt64();
float localPositionX = packetStream.ReadSingle();
float localPositionY = packetStream.ReadSingle();
float localPositionZ = packetStream.ReadSingle();
LocalPosition = new JVector(localPositionX, localPositionY, localPositionZ);
flag = packetStream.ReadBit();
if (flag)
{
float localLinearVelocityX = packetStream.ReadSingle();
float localLinearVelocityY = packetStream.ReadSingle();
float localLinearVelocityZ = packetStream.ReadSingle();
LocalLinearVelocity = new JVector(localLinearVelocityX, localLinearVelocityY, localLinearVelocityZ);
}
}
}
/// <summary>
/// Quaternions are added.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The sum of both quaternions.</param>
public static void Add(ref JQuaternion quaternion1, ref JQuaternion quaternion2, out JQuaternion result)
{
result.X = quaternion1.X + quaternion2.X;
result.Y = quaternion1.Y + quaternion2.Y;
result.Z = quaternion1.Z + quaternion2.Z;
result.W = quaternion1.W + quaternion2.W;
}
public static Quaternion Convert(JQuaternion q)
{
return(new Quaternion(q.X, q.Y, q.Z, q.W));
}
public static Quaternion ToQuaternion(this JMatrix matrix)
{
return(JQuaternion.CreateFromMatrix(matrix).ToQuaternion());
}
internal static Quaternion ToArenaData(JMatrix v)
{
var q = JQuaternion.CreateFromMatrix(v);
return(ToArenaData(q));
}
private void IntegrateCallback(object obj)
{
var body = obj as RigidBody;
JVector temp;
JVector.Multiply(ref body.linearVelocity, timestep, out temp);
JVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
JVector axis;
float angle = body.angularVelocity.Length();
if (angle < 0.001f)
{
JVector.Multiply(ref body.angularVelocity,
(0.5f*timestep - (timestep*timestep*timestep)*(0.020833333333f)*angle*angle),
out axis);
}
else
{
JVector.Multiply(ref body.angularVelocity, ((float) Math.Sin(0.5f*angle*timestep)/angle), out axis);
}
var dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (float) Math.Cos(angle*timestep*0.5f));
JQuaternion ornA;
JQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
JQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize();
JMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
if ((body.Damping & RigidBody.DampingType.Linear) != 0)
JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
if ((body.Damping & RigidBody.DampingType.Angular) != 0)
JVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
body.SweptExpandBoundingBox(timestep);
}
public static Quaternion ToQuaternion(this JQuaternion v)
{
return(new Quaternion(v.X, v.Y, v.Z, v.W));
}
public static void CreateFromYawPitchRoll(Fix64 yaw, Fix64 pitch, Fix64 roll, out JQuaternion result)
{
Fix64 num9 = roll * 0.5f;
Fix64 num6 = (Fix64)Fix64.Sin(num9);
Fix64 num5 = (Fix64)Fix64.Cos(num9);
Fix64 num8 = pitch * 0.5f;
Fix64 num4 = (Fix64)Fix64.Sin(num8);
Fix64 num3 = (Fix64)Fix64.Cos(num8);
Fix64 num7 = yaw * 0.5f;
Fix64 num2 = (Fix64)Fix64.Sin(num7);
Fix64 num = (Fix64)Fix64.Cos(num7);
result.X = ((num * num4) * num5) + ((num2 * num3) * num6);
result.Y = ((num2 * num3) * num5) - ((num * num4) * num6);
result.Z = ((num * num3) * num6) - ((num2 * num4) * num5);
result.W = ((num * num3) * num5) + ((num2 * num4) * num6);
}
/// <summary>
/// Quaternions are subtracted.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The difference of both quaternions.</param>
public static void Subtract(ref JQuaternion quaternion1, ref JQuaternion quaternion2, out JQuaternion result)
{
result.X = quaternion1.X - quaternion2.X;
result.Y = quaternion1.Y - quaternion2.Y;
result.Z = quaternion1.Z - quaternion2.Z;
result.W = quaternion1.W - quaternion2.W;
}