/// <summary>
/// Draws the twist constraints accoriding in Unity using Giszmos
/// </summary>
/// <param name="b">The bone with its constraints</param>
/// <param name="refBone">The to be twisted against</param>
/// <param name="poss">The position of where it should be drawn</param>
/// <param name="scale">The scale of the constraints</param>
public static void DrawTwistConstraints(Bone b, Bone refBone, OpenTK.Vector3 poss, float scale)
{
if (b.Orientation.Xyz.IsNaN() || refBone.Orientation.Xyz.IsNaN())
{
return;
}
OpenTK.Vector3 thisY = b.GetYAxis();
OpenTK.Quaternion referenceRotation = refBone.Orientation * b.ParentPointer;
OpenTK.Vector3 parentY = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitY, referenceRotation);
OpenTK.Vector3 parentZ = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitZ, referenceRotation);
OpenTK.Quaternion rot = QuaternionHelper2.GetRotationBetween(parentY, thisY);
OpenTK.Vector3 reference = OpenTK.Vector3.Transform(parentZ, rot);
reference.Normalize();
Debug.DrawRay(poss.Convert(), (b.GetZAxis() * scale * 2).Convert(), Color.cyan);
float startTwistLimit = OpenTK.MathHelper.DegreesToRadians(b.StartTwistLimit);
OpenTK.Vector3 m = OpenTK.Vector3.Transform(reference, OpenTK.Quaternion.FromAxisAngle(thisY, startTwistLimit));
m.Normalize();
Debug.DrawRay(poss.Convert(), m.Convert() * scale, Color.yellow);
float endTwistLimit = OpenTK.MathHelper.DegreesToRadians(b.EndTwistLimit);
OpenTK.Vector3 m2 = OpenTK.Vector3.Transform(reference, OpenTK.Quaternion.FromAxisAngle(thisY, endTwistLimit));
m2.Normalize();
Debug.DrawRay(poss.Convert(), m2.Convert() * scale, Color.magenta);
Debug.DrawLine((poss + (m * scale)).Convert(), (poss + (m2 * scale)).Convert(), Color.cyan);
}
/// <summary>
/// Using Unity Debug, draws the x,y,z axis of a Quaternion as x red, y green and z blue
/// </summary>
/// <param name="rot">The OpenTK Quaterion</param>
/// <param name="pos">The UnityEngine Vector3 position to be drawn</param>
/// <param name="scale">The float length of the axis in meter</param>
public static void DrawRays(OpenTK.Quaternion rot, Vector3 pos, float scale)
{
OpenTK.Vector3 right = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitX, rot);
OpenTK.Vector3 up = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitY, rot);
OpenTK.Vector3 forward = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitZ, rot);
Debug.DrawRay(pos, up.Convert() * scale, Color.green);
Debug.DrawRay(pos, right.Convert() * scale, Color.red);
Debug.DrawRay(pos, forward.Convert() * scale, Color.blue);
}
/// <summary>
/// Using Unity Debug, draws the x,y,z axis of a Quaternion as x magenta, y yellow and z cyan
/// </summary>
/// <param name="rot">The OpenTK Quaterion</param>
/// <param name="pos">The UnityEngine Vector3 position to be drawn</param>
/// <param name="scale">The float length of the axis in meter</param>
public static void DrawRays2(Quaternion rot, Vector3 pos, float scale)
{
OpenTK.Vector3 right = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitX, rot.Convert());
OpenTK.Vector3 up = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitY, rot.Convert());
OpenTK.Vector3 forward = OpenTK.Vector3.Transform(OpenTK.Vector3.UnitZ, rot.Convert());
Debug.DrawRay(pos, right.Convert() * scale, Color.magenta);
Debug.DrawRay(pos, up.Convert() * scale, Color.yellow);
Debug.DrawRay(pos, forward.Convert() * scale, Color.cyan);
}
/// <summary>
/// Using Unity Debug, draws the x,y,z axis of a Quaternion as x red, y green and z blue
/// </summary>
/// <param name="rot">The OpenTK Quaterion</param>
/// <param name="pos">The OpenTK Vector3 position to be drawn</param>
/// <param name="scale">The float length of the axis in meters</param>
public static void DrawRays(OpenTK.Quaternion rot, OpenTK.Vector3 pos, float scale)
{
DrawRays(rot, pos.Convert(), scale);
}
/// <summary>
/// Draws a line from two 3d points in Unity in a specific color
/// </summary>
/// <param name="start">The starting point as a OpenTK Vector3</param>
/// <param name="end">The end point as a OpenTK Vector3</param>
/// <param name="c">The color of the line as a UnityEngine Color</param>
public static void DrawLine(OpenTK.Vector3 start, OpenTK.Vector3 end, Color c)
{
Debug.DrawLine(start.Convert(), end.Convert(), c);
}
/// <summary>
/// Draws an array in unity using Unity.Debug
/// </summary>
/// <param name="pos">The staring position of the OpenTK Vector3</param>
/// <param name="dir">The direction of the vector of the OpenTK Vector3</param>
/// <param name="length">The length of the vector</param>
/// <param name="c">The color of the vector as a UnityEngine Color</param>
public static void DrawVector(OpenTK.Vector3 pos, OpenTK.Vector3 dir, float size, Color c)
{
Debug.DrawRay(pos.Convert(), Vector3.Normalize(dir.Convert()) * size, c);
}
/// <summary>
/// Draws an array in unity using Unity.Debug in black
/// </summary>
/// <param name="pos">The staring position of the OpenTK Vector3</param>
/// <param name="dir">The direction of the vector of the OpenTK Vector3</param>
/// <param name="length">The length of the vector</param>
public static void DrawVector(OpenTK.Vector3 pos, OpenTK.Vector3 dir, float length)
{
Debug.DrawRay(pos.Convert(), Vector3.Normalize(dir.Convert()) * length, Color.black);
}
/// <summary>
/// Draws an array in unity using Unity.Debug
/// </summary>
/// <param name="pos">The staring position of the OpenTK Vector3</param>
/// <param name="dir">The direction of the vector of the OpenTK Vector3</param>
/// <param name="c">The color the of the Vector as a UnityEngine Color</param>
public static void DrawVector(OpenTK.Vector3 pos, OpenTK.Vector3 dir, Color c)
{
Debug.DrawRay(pos.Convert(), dir.Convert(), c);
}
/// <summary>
/// Using Unity Debug, draws the x,y,z axis of a Quaternion as x magenta, y yellow and z cyan
/// </summary>
/// <param name="rot">The OpenTK Quaterion</param>
/// <param name="pos">The OpenTK Vector3 position to be drawn</param>
public static void DrawRays2(OpenTK.Quaternion rot, OpenTK.Vector3 pos)
{
DrawRays2(rot.Convert(), pos.Convert(), 0.07f);
}
/// <summary>
/// Using Unity Debug, draws the x,y,z axis of a Quaternion at 7 cm scale as x red, y green and z blue
/// </summary>
/// <param name="rot">The OpenTK Quaterion</param>
/// <param name="pos">The OpenTK Vector3 position to be drawn</param>
public static void DrawRays(OpenTK.Quaternion rot, OpenTK.Vector3 pos)
{
DrawRays(rot, pos.Convert());
}