private void UpdateMatrix(ref MotionSystems.FSMI_TopTableMatrixPhysical pos, float ax, float ay, float az, float dx, float dy, float dz)
{
float sinAX = UnityEngine.Mathf.Sin(ax);
float cosAX = UnityEngine.Mathf.Cos(ax);
float sinAY = UnityEngine.Mathf.Sin(ay);
float cosAY = UnityEngine.Mathf.Cos(ay);
float sinAZ = UnityEngine.Mathf.Sin(az);
float cosAZ = UnityEngine.Mathf.Cos(az);
pos.m11 = cosAY * cosAZ;
pos.m12 = cosAZ * sinAX * sinAY - cosAX * sinAZ;
pos.m13 = cosAX * cosAZ * sinAY + sinAX * sinAZ;
pos.m14 = dx;
pos.m21 = cosAY * sinAZ;
pos.m22 = cosAX * cosAZ + sinAX * sinAY * sinAZ;
pos.m23 = -cosAZ * sinAX + cosAX * sinAY * sinAZ;
pos.m24 = dy;
pos.m31 = -sinAY;
pos.m32 = cosAY * sinAX;
pos.m33 = cosAX * cosAY;
pos.m34 = dz;
pos.m41 = 0;
pos.m42 = 0;
pos.m43 = 0;
pos.m44 = 1;
} /* UpdateMatrix() */
} /* UpdateMatrix() */
public virtual void Update(float deltaTime, ref MotionSystems.FSMI_TopTableMatrixPhysical matrix)
{
// check first call
if (this.m_firstCall)
{
this.UpdateMatrix(
ref matrix,
0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f
);
// reset
this.m_firstCall = false;
// skip the rest
return;
} /* if() */
// update values + noise to avoid locking mechanism
this.UpdateMatrix(
ref matrix,
/* pitch in rad */
(
-1.0f
* (
this.m_behaviour.transform.localRotation.eulerAngles.x
/* sin noise */
+ (
/* noise amplitude in degree */ 0.05f
* UnityEngine.Mathf.Sin(UnityEngine.Time.fixedTime)
)
)
* UnityEngine.Mathf.Deg2Rad
),
/* roll in rad */
(
-1.0f
* (
this.m_behaviour.transform.localRotation.eulerAngles.z
// /* cos noise */
// + (
// /* noise amplitude in degree */ 0.03f
// * UnityEngine.Mathf.Cos(UnityEngine.Time.fixedTime * 100.0f)
// )
)
* UnityEngine.Mathf.Deg2Rad
),
/* yaw in rad */
(
(
this.m_behaviour.transform.localRotation.eulerAngles.y
// /* sin noise */
// + (
// /* noise amplitude in degree */ 0.05f
// * UnityEngine.Mathf.Sin(UnityEngine.Time.fixedTime * 100.0f)
// )
)
* UnityEngine.Mathf.Deg2Rad
),
/* sway in mm */
(
this.m_behaviour.transform.localPosition.x
* 1000.0f
),
/* surge in mm */
(
this.m_behaviour.transform.localPosition.z
* 1000.0f
),
/* heave in mm */
(
(
this.m_behaviour.transform.localPosition.y
// /* sin noise */
// + (
// /* noise amplitude in m */ 0.0005f
// * UnityEngine.Mathf.Sin(UnityEngine.Time.fixedTime * 50.0f)
// )
)
* 1000.0f
)
);
} /* Update() */
