Matrix33 Matrix33::Rotate(Vector3 const &aAxis, float const aAngle) const
{
// THE IDEA:
// Rotate to xz plane
// Rotate to z axis
// Rotate about z axis
// Rotate by inverse z axis transform
// Rotate by inverse xz plane transform
float angle = aAngle * DEGREE_TO_RADS;
float cosine = cos(angle);
float sine = sin(angle);
float xylength = sqrt(aAxis.x * aAxis.x + aAxis.y * aAxis.y);
float xyzlength = aAxis.length();
// What if we're already on the z plane?
float templength = xylength > 0.0f ? xylength : 1.0f;
float xzvalues[3][3] =
{
{ aAxis.x / templength, aAxis.y / templength, 0 },
{ -aAxis.y / templength, aAxis.x / templength, 0 },
{ 0, 0, 1 } };
float zvalues[3][3] =
{
{ aAxis.z / xyzlength, 0, -xylength / xyzlength },
{ 0, 1, 0 },
{ xylength / xyzlength, 0, aAxis.z / xyzlength } };
float rotvalues[3][3] =
{
{ cosine, -sine, 0 },
{ sine, cosine, 0 },
{ 0, 0, 1 } };
// Create identity matrix if on z plane already
if (xylength <= 0.0f)
{
xzvalues[0][0] = 1.0f;
xzvalues[1][1] = 1.0f;
}
Matrix33 ret;
Matrix33 xztrans(xzvalues);
Matrix33 ztrans(zvalues);
Matrix33 xzinvert = xztrans.Invert();
Matrix33 zinvert = ztrans.Invert();
Matrix33 rotation(rotvalues);
ret = xzinvert * zinvert * rotation * ztrans * xztrans;
return ret;
}
/**
* @brief Get all possible collisions with another line
* @param aCompare Line to check
* @param aOutput Output circle
* @return If circle is found or not
*/
bool Line::GetCollisions(Line const &aCompare, Circle &aOutput)
{
Vector3 d = aCompare.position - position;
float dist = d.length();
float longer = length >= aCompare.length ? length : aCompare.length;
float shorter = length >= aCompare.length ? aCompare.length : length;
print_line(*this);
print_line(aCompare);
// If they're in the exact same location that would be a problem
if (position == aCompare.position)
{
// IS THIS THE SAME EXACT SPHERE?!
if (length == aCompare.length)
{
aOutput.position = position;
aOutput.radius = length;
aOutput.up = Vector3(0, 1, 0);
aOutput.right = Vector3(0, 0, 1);
DebugLogPrint("Same exact sphere, gonna return false\n\n");
}
else
{
DebugLogPrint("One sphere is inside of another, WAT\n");
DebugLogPrint("Therefore, cannot touch, gonna return false\n\n");
}
print_circle(aOutput);
DebugLogPrint("\n\n");
return false;
}
// > is an early out
if (dist < length + aCompare.length && dist != longer - shorter)
{
// THE IDEA: Find one point, rotate about arbitrary axis between spheres
// Reduce the problem to solving a circle
Vector3 axis = d.normalize();
float xylength = sqrt(axis.x * axis.x + axis.y * axis.y);
float xyzlength = axis.length();
// Don't want to divide by zero
float templength = xylength > 0.0f ? xylength : 1.0f;
// Our matrices
// First, go to xz plane
float xzvalues[3][3] =
{
{ axis.x / templength, axis.y / templength, 0 },
{ -axis.y / templength, axis.x / templength, 0 },
{ 0, 0, 1 } };
// Then, only to z axis
float zvalues[3][3] =
{
{ axis.z / xyzlength, 0, -xylength / xyzlength },
{ 0, 1, 0 },
{ xylength / xyzlength, 0, axis.z / xyzlength } };
// Create identity matrix if we're already on the z plane
if (xylength <= 0.0f)
{
xzvalues[0][0] = 1.0f;
xzvalues[1][1] = 1.0f;
xzvalues[1][0] = 0.0f;
xzvalues[0][1] = 0.0f;
}
// Create our matrices and invert them
Matrix33 xztrans(xzvalues);
Matrix33 ztrans(zvalues);
Matrix33 toZAxis = ztrans * xztrans;
Matrix33 toZAxisInverse = toZAxis.Invert();
Vector3 comparePos = toZAxis * d;
dist = comparePos.length();
// Circle collision function
float z = ((dist * dist) - (aCompare.length * aCompare.length)
+ (length * length)) / (2.0f * dist);
float y = sqrt((length * length) - (z * z));
// Create our circle
aOutput.position = Vector3(0, 0, z);
aOutput.up = Vector3(0, 1, 0);
aOutput.right = Vector3(0, 0, 1);
aOutput.radius = y;
aOutput.position = toZAxisInverse * aOutput.position;
aOutput.position += position;
aOutput.up = toZAxisInverse * aOutput.up;
aOutput.right = toZAxisInverse * aOutput.right;
}
else if (dist == length + aCompare.length)
{
// They barely touch, from the outside
Vector3 largerPos =
length >= aCompare.length ? position : aCompare.position;
Vector3 smallerPos =
length >= aCompare.length ? aCompare.position : position;
float smallerLen = length >= aCompare.length ? aCompare.length : length;
d = smallerPos - largerPos;
DebugLogPrint("The radii barely touch, will still return true, but radius is zero\n");
aOutput.position = smallerPos - (d.normalize() * smallerLen);
aOutput.radius = 0;
aOutput.up = Vector3(0, 1, 0);
aOutput.right = Vector3(0, 0, 1);
}
else if (dist == longer - shorter)
{
// They barely touch, from the inside
Vector3 largerPos =
length >= aCompare.length ? position : aCompare.position;
Vector3 smallerPos =
length >= aCompare.length ? aCompare.position : position;
float smallerLen = length >= aCompare.length ? aCompare.length : length;
d = smallerPos - largerPos;
DebugLogPrint("The radii barely touch, will still return true, but radius is zero\n");
aOutput.position = smallerPos + (d.normalize() * smallerLen);
aOutput.radius = 0;
aOutput.up = Vector3(0, 1, 0);
aOutput.right = Vector3(0, 0, 1);
}
else
{
DebugLogPrint("These LineSegments can never touch. Returning false.\n\n");
return false;
}
print_circle(aOutput);
DebugLogPrint("\n\n");
return true;
}