/// <summary>
/// Set relative offset, based on an entity's offsets from the default positioning.
/// </summary>
/// <param name="relPos">The relative position.</param>
/// <param name="relQuat">The relative quaternion.</param>
public void SetRelativeForEntity(Location relPos, MathHelpers.Quaternion relQuat)
{
Entity.SetPosition(GetAccuratePosition(AttachedTo.LastKnownPosition, -Location.UnitZ) + relPos);
Entity.SetOrientation(relQuat);
SetRelativeToCurrent(Vector3.UnitY, -Location.UnitZ);
Tick();
}
/// <summary>
/// Sets the relative offset based on the attached properties.
/// </summary>
/// <param name="orient">The attached orientation.</param>
/// <param name="pos">The attached position.</param>
public void SetRelativeBasedOn(MathHelpers.Quaternion orient, Location pos)
{
Matrix worldTrans = Matrix.CreateFromQuaternion(orient.ToBEPU()) * Matrix.CreateTranslation(pos.ToBVector());
Matrix.Invert(ref worldTrans, out Matrix inverted);
RelativeOffset = Matrix.CreateFromQuaternion(Entity.LastKnownOrientation.ToBEPU()) * Matrix.CreateTranslation(Entity.LastKnownPosition.ToBVector()) * inverted;
}
/// <summary>
/// Sets this entity's position and orientation relative to <see cref="AttachedTo"/>.
/// </summary>
/// <param name="position">The attached-to entity's position.</param>
/// <param name="orient">The attached-to entity's orientation.</param>
public void SetPositionOrientation(Location position, MathHelpers.Quaternion orient)
{
Matrix worldTrans = Matrix.CreateFromQuaternion(orient.ToBEPU()) * Matrix.CreateTranslation(position.ToBVector());
Matrix tmat = RelativeOffset * worldTrans;
Location pos = new Location(tmat.Translation);
MathHelpers.Quaternion quat = BEPUutilities.Quaternion.CreateFromRotationMatrix(tmat).ToCore();
Entity.SetPosition(pos);
Entity.SetOrientation(quat);
}
/// <summary>
/// Updates the entity's local fields from spawned variant.
/// </summary>
public void UpdateFields()
{
InternalMass = SpawnedBody.Mass;
InternalGravity = new Location(SpawnedBody.Gravity.Value);
InternalFriction = SpawnedBody.Material.KineticFriction;
InternalBounciness = SpawnedBody.Material.Bounciness;
InternalLinearVelocity = new Location(SpawnedBody.LinearVelocity);
InternalAngularVelocity = new Location(SpawnedBody.AngularVelocity);
InternalPosition = new Location(SpawnedBody.Position);
InternalOrientation = SpawnedBody.Orientation.ToCore();
}
/// <summary>
/// Checks and handles an orientation update.
/// </summary>
/// <param name="q">The new orientation.</param>
public void OriCheck(MathHelpers.Quaternion q)
{
if (NoCheck)
{
return;
}
BEPUutilities.Quaternion qb = q.ToBEPU();
BEPUutilities.Quaternion qio = InternalOrientation.ToBEPU();
BEPUutilities.Quaternion.GetRelativeRotation(ref qb, ref qio, out BEPUutilities.Quaternion rel);
if (BEPUutilities.Quaternion.GetAngleFromQuaternion(ref rel) > 0.01) // TODO: Is this validation needed? This is very expensive to run.
{
Orientation = q;
}
}
/// <summary>
/// Ticks external positioning updates.
/// </summary>
public void TickUpdates()
{
NoCheck = CheckDisableAllowed;
Location bpos = new Location(SpawnedBody.Position);
if (InternalPosition.DistanceSquared(bpos) > 0.0001)
{
InternalPosition = bpos;
Location coff = new Location(BEPUutilities.Quaternion.Transform(Shape.GetCenterOffset(), SpawnedBody.Orientation));
Entity.OnPositionChanged?.Invoke((bpos - coff) * PhysicsWorld.RelativeScaleForward);
}
BEPUutilities.Quaternion cur = SpawnedBody.Orientation;
BEPUutilities.Quaternion qio = InternalOrientation.ToBEPU();
BEPUutilities.Quaternion.GetRelativeRotation(ref cur, ref qio, out BEPUutilities.Quaternion rel);
if (BEPUutilities.Quaternion.GetAngleFromQuaternion(ref rel) > 0.0001)
{
InternalOrientation = cur.ToCore();
Entity.OnOrientationChanged?.Invoke(cur.ToCore());
}
NoCheck = false;
}
// TODO: Damping values!
/// <summary>
/// Handles the physics entity being spawned into a world.
/// </summary>
public void SpawnHandle()
{
if (!GravityIsSet)
{
InternalGravity = PhysicsWorld.Gravity;
GravityIsSet = true;
}
if (Shape is EntityCharacterShape chr)
{
CharacterController cc = chr.GetBEPUCharacter();
cc.Tag = Entity;
OriginalObject = cc;
SpawnedBody = cc.Body;
SpawnedBody.Mass = InternalMass;
}
else
{
SpawnedBody = new Entity(Shape.GetBEPUShape(), InternalMass);
OriginalObject = SpawnedBody;
SpawnedBody.Orientation = InternalOrientation.ToBEPU();
}
SpawnedBody.LinearVelocity = InternalLinearVelocity.ToBVector();
SpawnedBody.AngularVelocity = InternalAngularVelocity.ToBVector();
SpawnedBody.Material.KineticFriction = InternalFriction;
SpawnedBody.Material.StaticFriction = InternalFriction;
SpawnedBody.Material.Bounciness = InternalBounciness;
SpawnedBody.Position = InternalPosition.ToBVector();
SpawnedBody.Gravity = InternalGravity.ToBVector();
SpawnedBody.Tag = Entity;
SpawnedBody.CollisionInformation.Tag = this;
// TODO: Other settings
PhysicsWorld.Spawn(Entity, OriginalObject);
Entity.OnTick += Tick;
InternalPosition = Location.Zero;
InternalOrientation = MathHelpers.Quaternion.Identity;
TickUpdates();
}
/// <summary>
/// Fixes this entity's orientation based on its attachment.
/// Does nothing for this property.
/// </summary>
public override void FixOrientation(MathHelpers.Quaternion orientation)
{
}
/// <summary>
/// Fixes this entity's orientation based on its attachment.
/// </summary>
public virtual void FixOrientation(MathHelpers.Quaternion orientation)
{
SetPositionOrientation(AttachedTo.LastKnownPosition, orientation);
}
