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