/// <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>
/// 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);
}