public PhysicsFpsFrameController()
{
TranslateX.Clear();
TranslateY.Clear();
TranslateZ.Clear();
RotateX.Clear();
RotateY.Clear();
RotateZ.Clear();
RotateX.Damp = 0.840f;
RotateY.Damp = 0.840f;
RotateZ.Damp = 0.840f;
RotateX.MaxDelta = 0.002f;
RotateY.MaxDelta = 0.002f;
RotateZ.MaxDelta = 0.001f;
TranslateX.Damp = 0.900f;
TranslateY.Damp = 0.900f;
TranslateZ.Damp = 0.900f;
TranslateX.MaxDelta = 0.020f;
TranslateY.MaxDelta = 0.020f;
TranslateZ.MaxDelta = 0.020f;
elevation = 0.0f;
heading = 0.0f;
UpdateTransform();
}
// Use this for initialization
void Start()
{
if (Target == null)
{
Target = GameObject.FindGameObjectWithTag("Player").transform;
}
Y = Body.GetComponent <RotateY>();
X = Gun.GetComponent <RotateX>();
Shoot = BulletSpawn.GetComponent <Shoot>();
X.rotationSpeed = RotateSpeed;
Y.rotationSpeed = RotateSpeed;
X.VisibleDistance = VisibleDistance;
Y.VisibleDistance = VisibleDistance;
X.UnvisibleDistance = UnvisibleDistance;
Y.UnvisibleDistance = UnvisibleDistance;
Shoot.UnvisibleDistance = UnvisibleDistance;
Shoot.Bullet = Bullet;
Shoot.ShootingSpeed = 1f / ShootsPerSecond;
X.target = Target;
Y.target = Target;
Shoot.target = Target;
Shoot.BulletDestroyTime = BulletDestroyTime;
Shoot.BulletSpeed = BulletSpeed;
}
public void RotationXaxisInverse()
{
Tuple p = Tuple.Point(0, 1, 0);
Matrix half_quarter = new RotateX(Math.PI / 4).GetTransform();
Matrix inv = half_quarter.Inverse();
Assert.AreEqual(inv * p, Tuple.Point(0, Math.Sqrt(2) / 2, -Math.Sqrt(2) / 2));
}
public void RotationXaxis()
{
Tuple p = Tuple.Point(0, 1, 0);
Matrix half_quarter = new RotateX(Math.PI / 4).GetTransform();
Matrix full_quarter = new RotateX(Math.PI / 2).GetTransform();
Assert.AreEqual(half_quarter * p, Tuple.Point(0, Math.Sqrt(2) / 2, Math.Sqrt(2) / 2));
Assert.AreEqual(full_quarter * p, Tuple.Point(0, 0, 1));
}
public void Clear()
{
TranslateX.Clear();
TranslateY.Clear();
TranslateZ.Clear();
RotateX.Clear();
RotateY.Clear();
RotateZ.Clear();
}
public void CombinedTransformations2()
{
Tuple p = Tuple.Point(1, 0, 1);
Matrix a = new RotateX(Math.PI / 2).GetTransform();
Matrix b = new Scale(5, 5, 5).GetTransform();
Matrix c = new Translate(10, 5, 7).GetTransform();
Matrix t = c * b * a;
Assert.AreEqual(t * p, Tuple.Point(15, 0, 7));
}
public void CombinedTransformations()
{
Tuple p = Tuple.Point(1, 0, 1);
Matrix a = new RotateX(Math.PI / 2).GetTransform();
Matrix b = new Scale(5, 5, 5).GetTransform();
Matrix c = new Translate(10, 5, 7).GetTransform();
Tuple p2 = a * p;
Assert.AreEqual(p2, Tuple.Point(1, -1, 0));
Tuple p3 = b * p2;
Assert.AreEqual(p3, Tuple.Point(5, -5, 0));
Tuple p4 = c * p3;
Assert.AreEqual(p4, Tuple.Point(15, 0, 7));
}
public HeadingElevationFrameController()
{
TranslateX.Clear();
TranslateY.Clear();
TranslateZ.Clear();
RotateX.Clear();
RotateY.Clear();
RotateZ.Clear();
#if false // 100 fps
RotateX.Damp = 0.950f;
RotateY.Damp = 0.950f;
RotateZ.Damp = 0.950f;
RotateX.MaxDelta = 0.002f;
RotateY.MaxDelta = 0.002f;
RotateZ.MaxDelta = 0.001f;
TranslateX.Damp = 0.950f;
TranslateY.Damp = 0.950f;
TranslateZ.Damp = 0.950f;
TranslateX.MaxDelta = 0.003f;
TranslateY.MaxDelta = 0.003f;
TranslateZ.MaxDelta = 0.003f;
#else // 120 fps
RotateX.Damp = 0.700f;
RotateY.Damp = 0.700f;
RotateZ.Damp = 0.700f;
RotateX.MaxDelta = 0.02f;
RotateY.MaxDelta = 0.02f;
RotateZ.MaxDelta = 0.02f;
TranslateX.Damp = 0.92f;
TranslateY.Damp = 0.92f;
TranslateZ.Damp = 0.92f;
TranslateX.MaxDelta = 0.004f;
TranslateY.MaxDelta = 0.004f;
TranslateZ.MaxDelta = 0.004f;
SpeedModifier.MaxValue = 3.0f;
SpeedModifier.Damp = 0.92f;
SpeedModifier.MaxDelta = 0.5f;
#endif
elevation = 0.0f;
heading = 0.0f;
Update();
/*Matrix4 elevationMatrix = Matrix4.CreateRotation(elevation, Vector3.UnitX);
* headingMatrix = Matrix4.CreateRotation(heading, Vector3.UnitY);
* rotationMatrix = headingMatrix * elevationMatrix;*/
}
protected Shape3C()
{
lines = new List <Line>();
invisibleLines = new List <Line>();
surfaces = new List <Surface>();
points = new List <Point>();
transformPoints = new List <Point>();
// transform matrix initialization
rotateX = new RotateX(0);
rotateY = new RotateY(0);
rotateZ = new RotateZ(0);
movePersp = new PerspectiveMove(60, 0, 0);
rotatePhi = new RotatePhi(0);
rotateTet = new RotateTheta(0);
perspectiveM = new Perspective(30);
move = new Move(0, 0, 0);
scale = new Scale(1);
}
public Rotator(Point p1, Point p2)
{
rotateX = new RotateX(0);
rotateY = new RotateY(0);
arotateX = new RotateX(0);
aroteteY = new RotateY(0);
rotateZ = new RotateZ(0);
//Point p1;
//Point p2;
//p1 = new Point(rp1);
//p2 = new Point(rp2);
move = new Move(-p1.rX, -p1.rY, -p1.rZ);
amove = new Move(p1.rX, p1.rY, p1.rZ);
// calculate rotate matrix
Point c;
if (p1.rZ > p2.rZ)
{
c = (p1 - p2);
}
else
{
c = (p2 - p1);
}
double k = Math.Sqrt(Math.Pow(c.rX, 2) + Math.Pow(c.rY, 2) + Math.Pow(c.rZ, 2));
c = c / k;
//Console.WriteLine("c:{0}", c);
double d = Math.Sqrt(Math.Pow(c.rY, 2) + Math.Pow(c.rZ, 2));
rotateX.angle = Math.Asin(c.rY / d);
rotateY.angle = -Math.Asin(c.rX);
arotateX.angle = -rotateX.angle;
aroteteY.angle = -rotateY.angle;
// Radians -> Degrees
//Func<double, double> RtG = (double r) => r * 180 / Math.PI;
}
public PhysicsFrameController()
{
TranslateX.Clear();
TranslateY.Clear();
TranslateZ.Clear();
RotateX.Clear();
RotateY.Clear();
RotateZ.Clear();
RotateX.Damp = 0.840f;
RotateY.Damp = 0.840f;
RotateZ.Damp = 0.840f;
RotateX.MaxDelta = 0.002f;
RotateY.MaxDelta = 0.002f;
RotateZ.MaxDelta = 0.001f;
TranslateX.Damp = 0.900f;
TranslateY.Damp = 0.900f;
TranslateZ.Damp = 0.900f;
TranslateX.MaxDelta = 0.020f;
TranslateY.MaxDelta = 0.020f;
TranslateZ.MaxDelta = 0.020f;
}
public FrameController()
{
TranslateX.Clear();
TranslateY.Clear();
TranslateZ.Clear();
RotateX.Clear();
RotateY.Clear();
RotateZ.Clear();
RotateX.Damp = 0.950f;
RotateY.Damp = 0.950f;
RotateZ.Damp = 0.950f;
RotateX.MaxDelta = 0.002f;
RotateY.MaxDelta = 0.002f;
RotateZ.MaxDelta = 0.001f;
TranslateX.Damp = 0.950f;
TranslateY.Damp = 0.950f;
TranslateZ.Damp = 0.950f;
TranslateX.MaxDelta = 0.003f;
TranslateY.MaxDelta = 0.003f;
TranslateZ.MaxDelta = 0.003f;
}
public void FixedUpdate()
{
TranslateX.Update();
TranslateY.Update();
TranslateZ.Update();
RotateX.Update();
RotateY.Update();
RotateZ.Update();
if (TranslateX.CurrentValue != 0.0f)
{
/* Right axis is column 0 */
positionInParent += rotationMatrix.GetColumn3(0) * TranslateX.CurrentValue;
}
if (TranslateY.CurrentValue != 0.0f)
{
/* Up axis is column 1 */
positionInParent += rotationMatrix.GetColumn3(1) * TranslateY.CurrentValue;
}
if (translateZ.CurrentValue != 0.0f)
{
/* View axis is column 2 */
positionInParent += rotationMatrix.GetColumn3(2) * TranslateZ.CurrentValue;
}
if (RotateX.CurrentValue != 0.0f)
{
Matrix4 rotation = Matrix4.CreateRotation(RotateX.CurrentValue, rotationMatrix.GetColumn3(0));
rotationMatrix = rotation * rotationMatrix;
}
if (RotateY.CurrentValue != 0.0f)
{
Matrix4 rotation = Matrix4.CreateRotation(RotateY.CurrentValue, rotationMatrix.GetColumn3(1));
rotationMatrix = rotation * rotationMatrix;
}
if (RotateZ.CurrentValue != 0.0f)
{
Matrix4 rotation = Matrix4.CreateRotation(RotateZ.CurrentValue, rotationMatrix.GetColumn3(2));
rotationMatrix = rotation * rotationMatrix;
}
}
public FrameController()
{
TranslateX.Clear();
TranslateY.Clear();
TranslateZ.Clear();
RotateX.Clear();
RotateY.Clear();
RotateZ.Clear();
#if false // 100 fps updates
RotateX.Damp = 0.950f;
RotateY.Damp = 0.950f;
RotateZ.Damp = 0.950f;
RotateX.MaxDelta = 0.002f;
RotateY.MaxDelta = 0.002f;
RotateZ.MaxDelta = 0.001f;
TranslateX.Damp = 0.950f;
TranslateY.Damp = 0.950f;
TranslateZ.Damp = 0.950f;
TranslateX.MaxDelta = 0.003f;
TranslateY.MaxDelta = 0.003f;
TranslateZ.MaxDelta = 0.003f;
#else
RotateX.Damp = 0.950f;
RotateY.Damp = 0.950f;
RotateZ.Damp = 0.950f;
RotateX.MaxDelta = 0.003f;
RotateY.MaxDelta = 0.003f;
RotateZ.MaxDelta = 0.002f;
TranslateX.Damp = 0.950f;
TranslateY.Damp = 0.950f;
TranslateZ.Damp = 0.950f;
TranslateX.MaxDelta = 0.010f;
TranslateY.MaxDelta = 0.010f;
TranslateZ.MaxDelta = 0.010f;
#endif
}
public void UpdateFixedStep()
{
TranslateX.Update();
TranslateY.Update();
TranslateZ.Update();
RotateX.Update();
RotateY.Update();
RotateZ.Update();
if (TranslateX.CurrentValue != 0.0f)
{
/* Right axis is column 0 */
positionInParent += rotationMatrix.GetColumn3(0) * TranslateX.CurrentValue;
}
if (TranslateY.CurrentValue != 0.0f)
{
/* Up axis is column 1 */
positionInParent += rotationMatrix.GetColumn3(1) * TranslateY.CurrentValue;
}
if (translateZ.CurrentValue != 0.0f)
{
/* View axis is column 2 */
positionInParent += rotationMatrix.GetColumn3(2) * TranslateZ.CurrentValue;
}
if (RotateX.CurrentValue != 0.0f)
{
Matrix4 rotation = Matrix4.CreateRotation(RotateX.CurrentValue, rotationMatrix.GetColumn3(0));
rotationMatrix = rotation * rotationMatrix;
}
if (RotateY.CurrentValue != 0.0f)
{
Matrix4 rotation = Matrix4.CreateRotation(RotateY.CurrentValue, rotationMatrix.GetColumn3(1));
rotationMatrix = rotation * rotationMatrix;
}
if (RotateZ.CurrentValue != 0.0f)
{
Matrix4 rotation = Matrix4.CreateRotation(RotateZ.CurrentValue, rotationMatrix.GetColumn3(2));
rotationMatrix = Matrix4.CreateRotation(RotateZ.CurrentValue, rotationMatrix.GetColumn3(2)) * rotationMatrix;
}
if (frame == null)
{
return;
}
Matrix4 localToParent = rotationMatrix;
Matrix4 parentToLocal;
Matrix4.Transpose(localToParent, out parentToLocal);
// HACK
if (positionInParent.Y < 0.03f)
{
positionInParent.Y = 0.03f;
}
/* Put translation to column 3 */
localToParent._03 = positionInParent.X;
localToParent._13 = positionInParent.Y;
localToParent._23 = positionInParent.Z;
localToParent._33 = 1.0f;
/* Put inverse translation to column 3 */
parentToLocal._03 = parentToLocal._00 * -positionInParent.X + parentToLocal._01 * -positionInParent.Y + parentToLocal._02 * -positionInParent.Z;
parentToLocal._13 = parentToLocal._10 * -positionInParent.X + parentToLocal._11 * -positionInParent.Y + parentToLocal._12 * -positionInParent.Z;
parentToLocal._23 = parentToLocal._20 * -positionInParent.X + parentToLocal._21 * -positionInParent.Y + parentToLocal._22 * -positionInParent.Z;
parentToLocal._33 = 1.0f;
//rame.LocalToParent.Set(localToParent);
Frame.LocalToParent.Set(localToParent, parentToLocal);
}
public void UpdateFixedStep()
{
TranslateX.Update();
TranslateY.Update();
TranslateZ.Update();
RotateX.Update();
RotateY.Update();
RotateZ.Update();
Matrix4 localToWorld = Matrix4.Identity;
localToWorld._00 = physicsObject.RigidBody.Orientation._00;
localToWorld._01 = physicsObject.RigidBody.Orientation._01;
localToWorld._02 = physicsObject.RigidBody.Orientation._02;
localToWorld._10 = physicsObject.RigidBody.Orientation._10;
localToWorld._11 = physicsObject.RigidBody.Orientation._11;
localToWorld._12 = physicsObject.RigidBody.Orientation._12;
localToWorld._20 = physicsObject.RigidBody.Orientation._20;
localToWorld._21 = physicsObject.RigidBody.Orientation._21;
localToWorld._22 = physicsObject.RigidBody.Orientation._22;
// We can't use friction because sphere shape will roll,
// and it won't do air resistance either
float damp = 0.98f;
float dampY = Services.Get <UserInterfaceManager>().FlyMode ? damp : 1.0f;
physicsObject.RigidBody.LinearVelocity = new Vector3(
physicsObject.RigidBody.LinearVelocity.X * damp,
physicsObject.RigidBody.LinearVelocity.Y * dampY,
physicsObject.RigidBody.LinearVelocity.Z * damp
);
float scale = 2.0f;
// N : Z back
// T : Y up
// B : X right
Vector3 back = localToWorld.GetColumn3(2);
back.Y = 0.0f;
back = Vector3.Normalize(back);
Vector3 up0 = Vector3.UnitY;
if (up0 == back)
{
up0 = back.MinAxis;
}
Vector3 right = Vector3.Normalize(Vector3.Cross(up0, back));
Vector3 up = Vector3.Cross(back, right);
physicsObject.RigidBody.IsActive = true;
//physicsObject.RigidBody.LinearVelocity += new Vector3(0.0f, 0.02f, 0.0f);
if (TranslateX.CurrentValue != 0.0f)
{
/* Right axis is column 0 */
physicsObject.RigidBody.IsActive = true;
//physicsObject.RigidBody.LinearVelocity += scale * localToWorld.GetColumn3(0) * TranslateX.CurrentValue;
physicsObject.RigidBody.LinearVelocity += scale * right * TranslateX.CurrentValue;
}
if (TranslateY.CurrentValue != 0.0f)
{
/* Up axis is column 1 */
physicsObject.RigidBody.IsActive = true;
//physicsObject.RigidBody.LinearVelocity += scale * localToWorld.GetColumn3(1) * TranslateY.CurrentValue;
physicsObject.RigidBody.LinearVelocity += scale * Vector3.UnitY * TranslateY.CurrentValue;
}
if (translateZ.CurrentValue != 0.0f)
{
/* Back axis is column 2 */
physicsObject.RigidBody.IsActive = true;
physicsObject.RigidBody.LinearVelocity += scale * back /*localToWorld.GetColumn3(2)*/ * TranslateZ.CurrentValue;
}
if (
(RotateX.CurrentValue != 0.0f) ||
(RotateY.CurrentValue != 0.0f)
)
{
physicsObject.RigidBody.IsActive = true;
float v = RotateY.CurrentValue;
float a = (float)System.Math.Abs(v);
float s = (float)System.Math.Sign(v);
float r = s * (float)System.Math.Pow(a, 0.2f);
//float r = RotateY.CurrentValue;
physicsObject.RigidBody.AngularVelocity += new Vector3(0.0f, r, 0.0f);
physicsObject.RigidBody.AngularVelocity *= 0.94f;
//Services.Instance.TextRenderer.DebugLine("Heading : " + heading + " Elevation : " + elevation);
//Services.Instance.TextRenderer.DebugLine("newLocalToWorld : " + newLocalToWorld);
//Services.Instance.TextRenderer.DebugLine("rotationMatrix : " + rotationMatrix);
}
physicsObject.RigidBody.IsActive = true;
}
public void UpdateFixedStep()
{
TranslateX.Update();
TranslateY.Update();
TranslateZ.Update();
RotateX.Update();
RotateY.Update();
RotateZ.Update();
Matrix4 localToWorld = Matrix4.Identity;
localToWorld._00 = physicsObject.RigidBody.Orientation._00;
localToWorld._01 = physicsObject.RigidBody.Orientation._01;
localToWorld._02 = physicsObject.RigidBody.Orientation._02;
localToWorld._10 = physicsObject.RigidBody.Orientation._10;
localToWorld._11 = physicsObject.RigidBody.Orientation._11;
localToWorld._12 = physicsObject.RigidBody.Orientation._12;
localToWorld._20 = physicsObject.RigidBody.Orientation._20;
localToWorld._21 = physicsObject.RigidBody.Orientation._21;
localToWorld._22 = physicsObject.RigidBody.Orientation._22;
// We can't use friction because sphere shape will roll,
// and it won't do air resistance either
float damp = 0.98f;
float dampY = Services.Get <UserInterfaceManager>().FlyMode ? damp : 1.0f;
physicsObject.RigidBody.LinearVelocity = new Vector3(
physicsObject.RigidBody.LinearVelocity.X * damp,
physicsObject.RigidBody.LinearVelocity.Y * dampY,
physicsObject.RigidBody.LinearVelocity.Z * damp
);
float scale = 2.0f;
// N : Z back
// T : Y up
// B : X right
Vector3 back = localToWorld.GetColumn3(2);
Vector3 up0 = Vector3.UnitY;
if (up0 == back)
{
up0 = back.MinAxis;
}
Vector3 right = Vector3.Normalize(Vector3.Cross(up0, back));
Vector3 up = Vector3.Cross(back, right);
//physicsObject.RigidBody.LinearVelocity += new Vector3(0.0f, 0.02f, 0.0f);
if (TranslateX.CurrentValue != 0.0f)
{
physicsObject.RigidBody.LinearVelocity += right * TranslateX.CurrentValue;
}
if (TranslateY.CurrentValue != 0.0f)
{
physicsObject.RigidBody.LinearVelocity += scale * Vector3.UnitY * TranslateY.CurrentValue;
}
if (translateZ.CurrentValue != 0.0f)
{
physicsObject.RigidBody.LinearVelocity += back * TranslateZ.CurrentValue;
}
// Compute desired orientation
Matrix4 lookAt = Matrix4.CreateLookAt(physicsObject.RigidBody.Position, Target, Vector3.UnitY);
// Required rotation to get to desired orientation
Matrix4 q = physicsObject.RigidBody.Orientation * Matrix4.Transpose(lookAt);
// Convert to axis angle
Vector3 axis;
float angle;
q.ToAxisAngle(out axis, out angle);
// Convert to torque
float k = 1.0f / (1.0f + angle / (float)System.Math.PI);
Matrix4 inertiaWorld = Matrix4.Invert(physicsObject.RigidBody.InverseInertiaWorld);
Vector3 torque = inertiaWorld.TransformDirection(axis);
Vector3 torqueScaled = torque * 400.0f * k * k;
physicsObject.RigidBody.AddTorque(torqueScaled);
// Also apply some damping
physicsObject.RigidBody.AngularVelocity *= 0.9f;
physicsObject.RigidBody.IsActive = true;
}
public void UpdateOncePerFrame()
{
if (physicsObject == null)
{
return;
}
TranslateX.Update();
TranslateY.Update();
TranslateZ.Update();
RotateX.Update();
RotateY.Update();
RotateZ.Update();
// We can't use friction because sphere shape will roll,
// and it won't do air resistance either
float damp = 0.88f;
float dampY = Services.Get <UserInterfaceManager>().FlyMode ? damp : 1.0f;
physicsObject.RigidBody.LinearVelocity = new Vector3(
physicsObject.RigidBody.LinearVelocity.X * damp,
physicsObject.RigidBody.LinearVelocity.Y * dampY,
physicsObject.RigidBody.LinearVelocity.Z * damp
);
float scale = 2.0f;
if (TranslateX.CurrentValue != 0.0f)
{
/* Right axis is column 0 */
//positionInParent += headingMatrix.GetColumn3(0) * TranslateX.CurrentValue;
physicsObject.RigidBody.IsActive = true;
physicsObject.RigidBody.LinearVelocity += scale * headingMatrix.GetColumn3(0) * TranslateX.CurrentValue;
}
if (TranslateY.CurrentValue != 0.0f)
{
/* Up axis is column 1 */
//positionInParent += headingMatrix.GetColumn3(1) * TranslateY.CurrentValue;
physicsObject.RigidBody.IsActive = true;
physicsObject.RigidBody.LinearVelocity += scale * headingMatrix.GetColumn3(1) * TranslateY.CurrentValue;
}
if (TranslateZ.CurrentValue != 0.0f)
{
/* View axis is column 2 */
//positionInParent += headingMatrix.GetColumn3(2) * TranslateZ.CurrentValue;
physicsObject.RigidBody.IsActive = true;
physicsObject.RigidBody.LinearVelocity += scale * headingMatrix.GetColumn3(2) * TranslateZ.CurrentValue;
}
if (
(RotateX.CurrentValue != 0.0f) ||
(RotateY.CurrentValue != 0.0f)
)
{
heading += RotateY.CurrentValue;
elevation += RotateX.CurrentValue;
UpdateTransform();
//Services.Instance.TextRenderer.DebugLine("Heading : " + heading + " Elevation : " + elevation);
//Services.Instance.TextRenderer.DebugLine("newLocalToWorld : " + newLocalToWorld);
//Services.Instance.TextRenderer.DebugLine("rotationMatrix : " + rotationMatrix);
}
}
public void UpdateFixedStep()
{
TranslateX.Update();
TranslateY.Update();
TranslateZ.Update();
RotateX.Update();
RotateY.Update();
RotateZ.Update();
SpeedModifier.Update();
float speed = 1.0f + speedModifier.CurrentValue;
if (TranslateX.CurrentValue != 0.0f)
{
/* Right axis is column 0 */
positionInParent += headingMatrix.GetColumn3(0) * TranslateX.CurrentValue * speed;
}
if (TranslateY.CurrentValue != 0.0f)
{
/* Up axis is column 1 */
positionInParent += headingMatrix.GetColumn3(1) * TranslateY.CurrentValue * speed;
}
if (translateZ.CurrentValue != 0.0f)
{
/* View axis is column 2 */
positionInParent += headingMatrix.GetColumn3(2) * TranslateZ.CurrentValue * speed;
}
if (
(RotateX.CurrentValue != 0.0f) ||
(RotateY.CurrentValue != 0.0f)
)
{
heading += RotateY.CurrentValue;
elevation += RotateX.CurrentValue;
Matrix4 elevationMatrix = Matrix4.CreateRotation(elevation, Vector3.UnitX);
headingMatrix = Matrix4.CreateRotation(heading, Vector3.UnitY);
rotationMatrix = headingMatrix * elevationMatrix;
}
if (frame == null)
{
return;
}
Matrix4 localToParent = rotationMatrix;
Matrix4 parentToLocal;
Matrix4.Transpose(localToParent, out parentToLocal);
// HACK
if (positionInParent.Y < 0.03f)
{
positionInParent.Y = 0.03f;
}
/* Put translation to column 3 */
localToParent._03 = positionInParent.X;
localToParent._13 = positionInParent.Y;
localToParent._23 = positionInParent.Z;
localToParent._33 = 1.0f;
/* Put inverse translation to column 3 */
parentToLocal._03 = parentToLocal._00 * -positionInParent.X + parentToLocal._01 * -positionInParent.Y + parentToLocal._02 * -positionInParent.Z;
parentToLocal._13 = parentToLocal._10 * -positionInParent.X + parentToLocal._11 * -positionInParent.Y + parentToLocal._12 * -positionInParent.Z;
parentToLocal._23 = parentToLocal._20 * -positionInParent.X + parentToLocal._21 * -positionInParent.Y + parentToLocal._22 * -positionInParent.Z;
parentToLocal._33 = 1.0f;
Frame.LocalToParent.Set(localToParent, parentToLocal);
}
