/// <summary>
/// Initialization of the rigid-body
/// </summary>
public override void Awake()
{
CalculateShape(ShapeType.Box);
RigidBody = new SteerableCollider(CollisionShape)
{
Position = Conversion.ToJitterVector(transform.position),
Orientation = JMatrix.CreateFromQuaternion(Conversion.ToJitterQuaternion(transform.rotation)),
CenterOfMass = CenterOfMass,
IsStatic = IsStatic,
IsActive = IsActive,
Tag = BodyTag.DrawMe,
Mass = 20.0f,
GameObject = gameObject,
SyncedObjects = SyncedObjects,
Material = new Jitter.Dynamics.Material {
KineticFriction = 1.0f, Restitution = 1.0f, StaticFriction = 1.0f
}
};
StartOrientation = RigidBody.Orientation;
StartPosition = RigidBody.Position;
PhysicsManager.Instance.World.AddBody(RigidBody);
}
/// <summary>
/// Initialization of the rigid-body
/// </summary>
public override void Awake()
{
CalculateShape(ShapeType);
RigidBody = new Collider(CollisionShape)
{
Position = Conversion.ToJitterVector(transform.position) + CenterOfMass,
Orientation = JMatrix.CreateFromQuaternion(Conversion.ToJitterQuaternion(transform.rotation)),
CenterOfMass = CenterOfMass,
IsStatic = IsStatic,
IsActive = IsActive,
IsAnimated = IsAnimated,
Tag = Tag,
PureCollider = PureCollider,
GameObject = gameObject,
SyncedObjects = new List <Follower>(),
Material = new Jitter.Dynamics.Material {
KineticFriction = 10.0f, Restitution = 0.0f, StaticFriction = 10.0f
},
Mass = 20.0f
};
StartPosition = RigidBody.Position;
StartOrientation = RigidBody.Orientation;
PhysicsManager.Instance.World.AddBody(RigidBody);
if (ShapeType == ShapeType.BoxInvisible)
{
gameObject.Model = null;
}
base.Awake();
}
public void RegisterDetail(DetailObject obj)
{
if (this.HasDetail(obj.ID))
{
throw new InvalidOperationException();
}
obj.Changed += Obj_Changed;
obj.InteractionTriggered += Obj_InterationTriggered;
this.details.Add(obj);
if (obj.Shape != null)
{
var body = new RigidBody(obj.Shape);
body.Position = obj.WorldPosition.Jitter();
body.Orientation = JMatrix.CreateFromQuaternion(obj.WorldRotation.Jitter());
body.IsStatic = true;
body.Tag = obj;
body.EnableDebugDraw = true;
body.Material = this.blockMaterial;
this.detailBodies.Add(obj.ID, body);
this.AddBody(body);
obj.PositionChanged += (s, e) =>
{
body.Position = obj.WorldPosition.Jitter();
body.Orientation = JMatrix.CreateFromQuaternion(obj.WorldRotation.Jitter());
};
}
this.OnDetailCreated(obj);
}
private void SetTransform(ScalableTransform t)
{
storedScale = t.Scale;
var tr = new Transform(t.Rotation, t.Translation);
var current = tr * baseTransform.Invert();
Body.Position = current.Translation.ToJVector();
Body.Orientation = JMatrix.CreateFromQuaternion(current.Rotation.ToQuaternion());
}
public void CopyState()
{
var position = gameObject.transform.position;
var rotation = gameObject.transform.rotation;
rigidBody.Position = new JVector(position.X, position.Y, position.Z);
// NOTE(francois): I tried using JMatrix.CreateFromAxisAngle and it did not work (both in rad and deg).
rigidBody.Orientation = JMatrix.CreateFromQuaternion(new JQuaternion(rotation.X, rotation.Y, rotation.Z, rotation.W));
rigidBody.IsStatic = isStatic;
}
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());
}
/// <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;
}
}
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);
}
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);
}
private BatchedControlEntity BatchPhysics(IGameContext gameContext, INode node, BatchedControlEntity entity)
{
var physicsComponentsToProcess = new List <Tuple <INode, PhysicalBaseRigidBodyComponent> >();
FindPhysicsComponentsUnderNode(node, physicsComponentsToProcess);
var transformedShapes = new List <CompoundShape.TransformedShape>();
foreach (var pair in physicsComponentsToProcess)
{
foreach (var body in pair.Item2.RigidBodies)
{
transformedShapes.Add(new CompoundShape.TransformedShape(
body.Shape,
JMatrix.CreateFromQuaternion(pair.Item2.FinalTransform.AbsoluteRotation.ToJitterQuaternion()),
pair.Item2.FinalTransform.AbsolutePosition.ToJitterVector()));
}
}
if (transformedShapes.Count == 0)
{
return(entity);
}
var compoundShape = new CompoundShape(transformedShapes);
if (entity == null)
{
entity = CreateBatchedControlEntity();
}
foreach (var pair in physicsComponentsToProcess)
{
pair.Item2.SetBatchedEntity(entity);
}
entity.AttachBatchedPhysics(compoundShape);
_hierarchy.MoveNode(node, _hierarchy.Lookup(entity));
_consoleHandle.LogInfo("Batching physics objected combined " + transformedShapes.Count + " shapes.");
return(entity);
}
/// <summary>
/// Single threaded.
/// </summary>
/// <param name="spawnable_idx"></param>
/// <returns></returns>
protected KeyValuePair <int, RigidBody> SpawnObject(int spawnable_idx, JVector?pos = null, JVector?vel = null, JQuaternion?ori = null)
{
var rb = new RigidBody(spawnable_rigid_bodies[spawnable_idx].Shape);
if (pos.HasValue)
{
rb.Position = pos.Value;
}
if (vel.HasValue)
{
rb.LinearVelocity = vel.Value;
}
if (ori.HasValue)
{
rb.Orientation = JMatrix.CreateFromQuaternion(ori.Value);
}
for (;;)
{
var id = random.Next(startobject_high_edge, int.MaxValue);
if (!objects.ContainsKey(id))
{
add_to_world(id, spawnable_idx, rb);
var ev = new Event3D
{
ObjectNotification = new Event3D_ObjectNotification
{
Id = id,
SpawnId = spawnable_idx,
Spawn = true,
Transform = ArenaServiceConv.ToArenaTransform(rb)
}
};
send_to_all_players(ev);
return(new KeyValuePair <int, RigidBody>(id, rb));
}
}
}
public static JMatrix ToJMatrix(this Quaternion rot)
{
return(JMatrix.CreateFromQuaternion(rot.ToJQuaternion()));
}
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 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();
}
private static JMatrix to_j(Quaternion r)
{
var q = new JQuaternion(r.X, r.Y, r.Z, r.W);
return(JMatrix.CreateFromQuaternion(q));
}