public virtual void DrawCapsule(float radius, float halfHeight, int upAxis, ref IndexedMatrix transform, ref IndexedVector3 color)
{
IndexedVector3 capStart = IndexedVector3.Zero;;
capStart[upAxis] = -halfHeight;
IndexedVector3 capEnd = IndexedVector3.Zero;
capEnd[upAxis] = halfHeight;
// Draw the ends
{
IndexedMatrix childTransform = transform;
childTransform._origin = transform * capStart;
DrawSphere(radius, ref childTransform, ref color);
}
{
IndexedMatrix childTransform = transform;
childTransform._origin = transform * capEnd;
DrawSphere(radius, ref childTransform, ref color);
}
// Draw some additional lines
IndexedVector3 start = transform._origin;
capStart[(upAxis + 1) % 3] = radius;
capEnd[(upAxis + 1) % 3] = radius;
DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color);
capStart[(upAxis + 1) % 3] = -radius;
capEnd[(upAxis + 1) % 3] = -radius;
DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color);
capStart[(upAxis + 2) % 3] = radius;
capEnd[(upAxis + 2) % 3] = radius;
DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color);
capStart[(upAxis + 2) % 3] = -radius;
capEnd[(upAxis + 2) % 3] = -radius;
DrawLine(start + transform._basis * capStart, start + transform._basis * capEnd, color);
}
public virtual void DrawCone(float radius, float height, int upAxis, ref IndexedMatrix transform, ref IndexedVector3 color)
{
IndexedVector3 start = transform._origin;
IndexedVector3 offsetHeight = IndexedVector3.Zero;
offsetHeight[upAxis] = height * 0.5f;
IndexedVector3 offsetRadius = IndexedVector3.Zero;
offsetRadius[(upAxis + 1) % 3] = radius;
IndexedVector3 offset2Radius = IndexedVector3.Zero;
offsetRadius[(upAxis + 2) % 3] = radius;
DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight + offsetRadius, color);
DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight - offsetRadius, color);
DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight + offset2Radius, color);
DrawLine(start + transform._basis * offsetHeight, start + transform._basis * -offsetHeight - offset2Radius, color);
}
public virtual void DrawBox(ref IndexedVector3 bbMin, ref IndexedVector3 bbMax, ref IndexedMatrix trans, ref IndexedVector3 color)
{
DrawLine(trans * bbMin, trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * bbMin, color);
DrawLine(trans * bbMin, trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * bbMax, color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), trans * bbMax, color);
DrawLine(trans * bbMax, trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color);
DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color);
}
public virtual void DrawPlane(ref IndexedVector3 planeNormal, float planeConst, ref IndexedMatrix transform, ref IndexedVector3 color)
{
IndexedVector3 planeOrigin = planeNormal * planeConst;
IndexedVector3 vec0, vec1;
TransformUtil.PlaneSpace1(ref planeNormal, out vec0, out vec1);
float vecLen = 100f;
IndexedVector3 pt0 = planeOrigin + vec0 * vecLen;
IndexedVector3 pt1 = planeOrigin - vec0 * vecLen;
IndexedVector3 pt2 = planeOrigin + vec1 * vecLen;
IndexedVector3 pt3 = planeOrigin - vec1 * vecLen;
DrawLine(transform * pt0, transform * pt1, color);
DrawLine(transform * pt2, transform * pt3, color);
}
public virtual void DrawSphere(ref IndexedVector3 p, float radius, ref IndexedVector3 color)
{
IndexedMatrix tr = IndexedMatrix.CreateTranslation(p);
DrawSphere(radius, ref tr, ref color);
}
/**@brief diagonalizes this matrix by the Jacobi method.
* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
* coordinate system, i.e., old_this = rot * new_this * rot^T.
* @param threshold See iteration
* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
* by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
*
* Note that this matrix is assumed to be symmetric.
*/
public void Diagonalize(out IndexedMatrix rot, float threshold, int maxSteps)
{
rot = IndexedMatrix.Identity;
for (int step = maxSteps; step > 0; step--)
{
// find off-diagonal element [p][q] with largest magnitude
int p = 0;
int q = 1;
int r = 2;
float max = Math.Abs(_el0.Y);
float v = Math.Abs(_el0.Z);
if (v > max)
{
q = 2;
r = 1;
max = v;
}
v = Math.Abs(_el1.Z);
if (v > max)
{
p = 1;
q = 2;
r = 0;
max = v;
}
float t = threshold * (Math.Abs(_el0.X) + Math.Abs(_el1.Y) + Math.Abs(_el2.Z));
if (max <= t)
{
if (max <= MathUtil.SIMD_EPSILON * t)
{
return;
}
step = 1;
}
// compute Jacobi rotation J which leads to a zero for element [p][q]
float mpq = this[p][q];
float theta = (this[q][q] - this[p][p]) / (2 * mpq);
float theta2 = theta * theta;
float cos;
float sin;
if (theta2 * theta2 < (10.0f / MathUtil.SIMD_EPSILON))
{
t = (theta >= 0.0f) ? (1.0f / (float)(theta + (float)Math.Sqrt(1 + theta2)))
: (1 / (theta - (float)Math.Sqrt(1 + theta2)));
cos = 1.0f / (float)Math.Sqrt(1 + t * t);
sin = cos * t;
}
else
{
// approximation for large theta-value, i.e., a nearly diagonal matrix
t = 1 / (theta * (2 + 0.5f / theta2));
cos = 1 - 0.5f * t * t;
sin = cos * t;
}
// apply rotation to matrix (this = J^T * this * J)
this[p, q] = 0;
this[q, p] = 0;
this[p, p] -= t * mpq;
this[q, q] += t * mpq;
float mrp = this[r][p];
float mrq = this[r][q];
this[r, p] = this[p, r] = cos * mrp - sin * mrq;
this[r, q] = this[q, r] = cos * mrq + sin * mrp;
// apply rotation to rot (rot = rot * J)
for (int i = 0; i < 3; i++)
{
mrp = this[i, p];
mrq = this[i, q];
this[i, p] = cos * mrp - sin * mrq;
this[i, q] = cos * mrq + sin * mrp;
}
}
}
