internal void Point(float3 x, float size, Unity.DebugDisplay.ColorIndex color)
{
var lines = new Unity.DebugDisplay.Lines(3);
lines.Draw(x - new float3(size, 0, 0), x + new float3(size, 0, 0), color);
lines.Draw(x - new float3(0, size, 0), x + new float3(0, size, 0), color);
lines.Draw(x - new float3(0, 0, size), x + new float3(0, 0, size), color);
}
internal void Draw(float3 x, float3 v, Unity.DebugDisplay.ColorIndex color)
{
var X0 = x;
var X1 = x + v;
m_Lines.Draw(X0, X1, color);
float3 dir;
float length = Physics.Math.NormalizeWithLength(v, out dir);
float3 perp, perp2;
Physics.Math.CalculatePerpendicularNormalized(dir, out perp, out perp2);
float3 scale = length * 0.2f;
m_Lines.Draw(X1, X1 + (perp - dir) * scale, color);
m_Lines.Draw(X1, X1 - (perp + dir) * scale, color);
m_Lines.Draw(X1, X1 + (perp2 - dir) * scale, color);
m_Lines.Draw(X1, X1 - (perp2 + dir) * scale, color);
perp *= length;
perp2 *= length;
m_Lines.Draw(X0 + perp + perp2, X0 + perp - perp2, color);
m_Lines.Draw(X0 + perp - perp2, X0 - perp - perp2, color);
m_Lines.Draw(X0 - perp - perp2, X0 - perp + perp2, color);
m_Lines.Draw(X0 - perp + perp2, X0 + perp + perp2, color);
}
internal void Draw(float3 center, float3 normal, float3 arm, float angle, Unity.DebugDisplay.ColorIndex color)
{
quaternion q = quaternion.AxisAngle(normal, angle / res);
float3 currentArm = arm;
m_Lines.Draw(center, center + currentArm, color);
for (int i = 0; i < res; i++)
{
float3 nextArm = math.mul(q, currentArm);
m_Lines.Draw(center + currentArm, center + nextArm, color);
currentArm = nextArm;
}
m_Lines.Draw(center, center + currentArm, color);
}
internal void Draw(float3 point, float3 axis, float angle, Unity.DebugDisplay.ColorIndex color)
{
float3 dir;
float scale = Physics.Math.NormalizeWithLength(axis, out dir);
float3 arm;
{
float3 perp1, perp2;
Physics.Math.CalculatePerpendicularNormalized(dir, out perp1, out perp2);
arm = math.mul(quaternion.AxisAngle(perp1, angle), dir) * scale;
}
quaternion q = quaternion.AxisAngle(dir, 2.0f * (float)math.PI / res);
for (int i = 0; i < res; i++)
{
float3 nextArm = math.mul(q, arm);
m_Lines.Draw(point, point + arm, color);
m_Lines.Draw(point + arm, point + nextArm, color);
arm = nextArm;
}
}
internal void Draw(float3 Size, float3 Center, quaternion Orientation, Unity.DebugDisplay.ColorIndex color)
{
float3x3 mat = math.float3x3(Orientation);
float3 x = mat.c0 * Size.x * 0.5f;
float3 y = mat.c1 * Size.y * 0.5f;
float3 z = mat.c2 * Size.z * 0.5f;
float3 c0 = Center - x - y - z;
float3 c1 = Center - x - y + z;
float3 c2 = Center - x + y - z;
float3 c3 = Center - x + y + z;
float3 c4 = Center + x - y - z;
float3 c5 = Center + x - y + z;
float3 c6 = Center + x + y - z;
float3 c7 = Center + x + y + z;
m_Lines.Draw(c0, c1, color); // ring 0
m_Lines.Draw(c1, c3, color);
m_Lines.Draw(c3, c2, color);
m_Lines.Draw(c2, c0, color);
m_Lines.Draw(c4, c5, color); // ring 1
m_Lines.Draw(c5, c7, color);
m_Lines.Draw(c7, c6, color);
m_Lines.Draw(c6, c4, color);
m_Lines.Draw(c0, c4, color); // between rings
m_Lines.Draw(c1, c5, color);
m_Lines.Draw(c2, c6, color);
m_Lines.Draw(c3, c7, color);
}