Пример #1
0
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;
}
Пример #2
0
/**
 * @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;
}