/* Draw an axis-aligned box located at the origin */
public static void DrawBox(Pose pose, float xLength, float yLength, float zLength)
{
float x2 = xLength / 2.0f;
float y2 = yLength / 2.0f;
float z2 = zLength / 2.0f;
DrawBox(pose, Vector3.Create(-x2, -y2, -z2), Vector3.Create(x2, y2, z2));
}
/* Draw an axis-aligned box off the origin*/
public static void DrawBox(Pose pose, Vector3 min, Vector3 max)
{
if (_renderer != null)
{
Position p0, p1, p2, p3, p4, p5, p6, p7;
if (pose != null)
{
p0 = pose.Orientation * min + pose.Position;
p1 = pose.Orientation * Vector3.Create(max.X, min.Y, min.Z) + pose.Position;
p2 = pose.Orientation * Vector3.Create(max.X, max.Y, min.Z) + pose.Position;
p3 = pose.Orientation * Vector3.Create(min.X, max.Y, min.Z) + pose.Position;
p4 = pose.Orientation * Vector3.Create(min.X, min.Y, max.Z) + pose.Position;
p5 = pose.Orientation * Vector3.Create(max.X, min.Y, max.Z) + pose.Position;
p6 = pose.Orientation * max + pose.Position;
p7 = pose.Orientation * Vector3.Create(min.X, max.Y, max.Z) + pose.Position;
}
else
{
p0 = Position.Create(min.X, min.Y, min.Z);
p1 = Position.Create(max.X, min.Y, min.Z);
p2 = Position.Create(max.X, max.Y, min.Z);
p3 = Position.Create(min.X, max.Y, min.Z);
p4 = Position.Create(min.X, min.Y, max.Z);
p5 = Position.Create(max.X, min.Y, max.Z);
p6 = Position.Create(max.X, max.Y, max.Z);
p7 = Position.Create(min.X, max.Y, max.Z);
}
DrawLine(p0, p1);
DrawLine(p1, p2);
DrawLine(p2, p3);
DrawLine(p3, p0);
DrawLine(p1, p5);
DrawLine(p5, p6);
DrawLine(p6, p2);
DrawLine(p5, p4);
DrawLine(p4, p7);
DrawLine(p7, p6);
DrawLine(p4, p0);
DrawLine(p7, p3);
}
}
public static void DrawPoint(Pose pose, float x, float y, float z)
{
if (_renderer != null)
{
Position pos;
if (pose != null)
pos = pose.Orientation * Vector3.Create(x, y, z) + pose.Position;
else
pos = Position.Create(x, y, z);
_renderer.DrawPoint(pos);
}
}
