vector.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

      q.y = (rot[1] + rot[3]) * temp;
      q.z = (rot[2] + rot[6]) * temp;
      break;
    case 3:
      q.y = sqrt(q.y);
      temp = 1.0/(4.0*q.y);
      q.y = (rot[2] - rot[6]) * temp;
      q.x = (rot[1] + rot[3]) * temp;
      q.z = (rot[5] + rot[7]) * temp;
      break;
    case 4:
      q.z = sqrt(q.z);
      temp = 1.0/(4.0*q.z);
      q.u = (rot[3] - rot[1]) * temp;
      q.x = (rot[2] + rot[6]) * temp;
      q.y = (rot[5] + rot[7]) * temp;
      break;
  }

  return q;
}
*/


/* REMOVE
//////////////////////////////////////////////////////////////////////////////
//  Create a quaternion from a pair of points.
GzQuatern GzQuaternFromLookAt(GzVector center, GzVector up)
{
  GzQuatern a;
  GzVector x, y, z;
  double R[9];

  x = GzVectorUnit(center);
  y = GzVectorCross(GzVectorUnit(up), x);
  z = GzVectorCross(x, y);

  R[0] = x.x;
  R[3] = x.y;
  R[6] = x.z;

  R[1] = y.x;
  R[4] = y.y;
  R[7] = y.z;
  
  R[2] = z.x;
  R[5] = z.y;
  R[8] = z.z;

  a = GzQuaternFromRot(R);

  return a;
}
*/


//////////////////////////////////////////////////////////////////////////////
// Invert a quaternion
GzQuatern GzQuaternInverse(GzQuatern a)
{
  GzQuatern b;

  b.u = a.u;
  b.x = -a.x;
  b.y = -a.y;
  b.z = -a.z;
  
  return b;
}


//////////////////////////////////////////////////////////////////////////////
// Normalize a quaternion
GzQuatern GzQuaternNormal(GzQuatern a)
{
  GzQuatern b;
  double s;

  s = sqrt(a.u * a.u + a.x * a.x + a.y * a.y + a.z * a.z);

  b.u = a.u / s;
  b.x = a.x / s;
  b.y = a.y / s;
  b.z = a.z / s;

  return b;
}


//////////////////////////////////////////////////////////////////////////////
// Multiple two quaternions
GzQuatern GzQuaternMul(GzQuatern a, GzQuatern b)
{
  GzQuatern c;

  c.u = a.u * b.u - a.x * b.x - a.y * b.y - a.z * b.z;
  c.x = a.u * b.x + a.x * b.u + a.y * b.z - a.z * b.y;
  c.y = a.u * b.y - a.x * b.z + a.y * b.u + a.z * b.x;
  c.z = a.u * b.z + a.x * b.y - a.y * b.x + a.z * b.u;
  
  return c;
}


//////////////////////////////////////////////////////////////////////////////
// Scale a quaternion rotation
GzQuatern GzQuaternScale(double s, GzQuatern a)
{
  GzQuatern b;

  // Convert to axis-and-angle
  b = GzQuaternToAxis(a);

  // Scale angle
  b.u *= s;

  // Convert back again
  return GzQuaternFromAxis(b.x, b.y, b.z, b.u);
}



//////////////////////////////////////////////////////////////////////////////
// See if a quatern is finite (e.g., not nan)
bool GzQuaternFinite(GzQuatern a)
{
  return finite(a.x) && finite(a.y) && finite(a.z) && finite(a.u);
}


//////////////////////////////////////////////////////////////////////////////
// Create a pose from the given position and rotation
GzPose GzPoseSet(GzVector pos, GzQuatern rot)
{
  GzPose p;

  p.pos = pos;
  p.rot = rot;

  return p;
}


//////////////////////////////////////////////////////////////////////////////
//  Create a pose from a triplet of vectors
GzPose GzPosePointAt(GzVector pos, GzVector at, GzVector up)
{
  GzPose pose;
  GzQuatern b;
  GzVector x, y, z;
  double R[3][3];

  x = GzVectorUnit(GzVectorSub(at, pos));
  y = GzVectorUnit(GzVectorCross(GzVectorUnit(up), x));
  z = GzVectorCross(x, y);

  R[0][0] = x.x;
  R[1][0] = x.y;
  R[2][0] = x.z;

  R[0][1] = y.x;
  R[1][1] = y.y;
  R[2][1] = y.z;
  
  R[0][2] = z.x;
  R[1][2] = z.y;
  R[2][2] = z.z;

  // TODO: switch cases to avoid instability
  b.u = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2;
  if (b.u < 1e-16)
    PRINT_WARN("pointing singularity (gimbal lock)"); 
  b.x = (R[2][1] - R[1][2]) / (4 * b.u);
  b.y = (R[0][2] - R[2][0]) / (4 * b.u);
  b.z = (R[1][0] - R[0][1]) / (4 * b.u);
  b = GzQuaternNormal(b);

  /*
  printf("x = %f %f %f  \n", x.x, x.y, x.z);
  printf("y = %f %f %f  \n", y.x, y.y, y.z);
  printf("z = %f %f %f  \n", z.x, z.y, z.z);
  printf("%f  \n", b.u);
  */

  pose.pos = pos;
  pose.rot = b;

  return pose;
}



//////////////////////////////////////////////////////////////////////////////
// See if a pose is finite (e.g., not nan)
bool GzPoseFinite(GzPose a)
{
  return GzVectorFinite(a.pos) && GzQuaternFinite(a.rot);
}


//////////////////////////////////////////////////////////////////////////////
// Add one pose to another: c = b + a
GzPose GzCoordPoseAdd(GzPose bpose, GzPose apose)
{
  GzPose cpose;

  cpose.pos = GzCoordPositionAdd(bpose.pos, apose.pos, apose.rot);
  cpose.rot = GzCoordRotationAdd(bpose.rot, apose.rot);

  return cpose;
}


//////////////////////////////////////////////////////////////////////////////
// Subtract one pose from another: c = b - a
GzPose GzCoordPoseSub(GzPose bpose, GzPose apose)
{
  GzPose cpose;

  cpose.pos = GzCoordPositionSub(bpose.pos, apose.pos, apose.rot);
  cpose.rot = GzCoordRotationSub(bpose.rot, apose.rot);

  return cpose;
}


//////////////////////////////////////////////////////////////////////////////
// Find the inverse of a pose; i.e., if b = ba + a, given b and ba,
// find a
GzPose GzCoordPoseSolve(GzPose ba, GzPose b)
{
  GzQuatern q;
  GzPose a;

  a.rot = GzQuaternMul(GzQuaternInverse(ba.rot), b.rot);                       
  q = GzQuaternMul(a.rot, GzQuaternSetVector(ba.pos));
  q = GzQuaternMul(q, GzQuaternInverse(a.rot));
  a.pos = GzVectorSub(b.pos, GzVectorSetQuatern(q));
  
  return a;
}

  
//////////////////////////////////////////////////////////////////////////////
// Add one position to another: c = b + a
GzVector GzCoordPositionAdd(GzVector bpos, GzVector apos, GzQuatern arot)
{
  GzVector cpos;
  
  // cpos = apos + arot * bpos * arot!
  cpos = GzVectorSetQuatern(GzQuaternMul(arot, GzQuaternMul(GzQuaternSetVector(bpos), GzQuaternInverse(arot))));
  cpos = GzVectorAdd(apos, cpos);

  return cpos;
}


//////////////////////////////////////////////////////////////////////////////
// Subtract one position from another: c = b - a
GzVector GzCoordPositionSub(GzVector bpos, GzVector apos, GzQuatern arot)
{
  GzVector cpos;

  // cpos = arot! * (bpos - apos) * arot
  cpos = GzVectorSub(bpos, apos);
  cpos = GzVectorSetQuatern(
      GzQuaternMul(GzQuaternInverse(arot), 
                   GzQuaternMul(GzQuaternSetVector(cpos), arot)
                  )
      );

  return cpos;
}


//////////////////////////////////////////////////////////////////////////////
// Add one rotation to another: b = r + a
GzQuatern GzCoordRotationAdd(GzQuatern r, GzQuatern a)
{
  GzQuatern b;

  // b = a * r
  b = GzQuaternMul(a, r);

  return b;
}


//////////////////////////////////////////////////////////////////////////////
// Subtract one rotation from another: r = b - a
GzQuatern GzCoordRotationSub(GzQuatern b, GzQuatern a)
{
  GzQuatern r;

  // r = a! * b
  r = GzQuaternMul(GzQuaternInverse(a), b);

  return r;
}

//////////////////////////////////////////////////////////////////////////////
// Compute a homogeneous transform matrix from a Quatern
GzHomo GzHomoFromQuatern(GzQuatern q)
{
  GzHomo b;

  b.m[0][0] = 1 - (2*(q.y*q.y) + 2*(q.z*q.z));
  b.m[0][1] = 2 * q.x * q.y + 2 * q.z * q.u;
  b.m[0][2] = 2 * q.x * q.z - 2 * q.y * q.u;

  b.m[1][0] = 2 * q.x * q.y - 2 * q.z * q.u;
  b.m[1][1] = 1 - (2*(q.x*q.x) + 2*(q.z*q.z));
  b.m[1][2] = 2 * q.y * q.z + 2 * q.x * q.u;

  b.m[2][0] = 2 * q.x * q.z + 2 * q.y * q.u;
  b.m[2][1] = 2 * q.y * q.z - 2 * q.x * q.u;
  b.m[2][2] = 1 - (2*(q.x*q.x) + 2*(q.y*q.y));

  return b;
}


//////////////////////////////////////////////////////////////////////////////
// Compute the inverse of the given transform matrix
GzHomo GzHomoInverse(GzHomo a)
{
  double det;
  GzHomo b;

  det = ( + a.m[0][0] * (a.m[1][1] * a.m[2][2] - a.m[1][2] * a.m[2][1]) - a.m[0][1] * (a.m[1][0] * a.m[2][2] - a.m[1][2] * a.m[2][0]) + a.m[0][2] * (a.m[1][0] * a.m[2][1] - a.m[1][1] * a.m[2][0]));
  b.m[0][0] = ( + (a.m[1][1] * a.m[2][2] - a.m[1][2] * a.m[2][1])) / det;
  b.m[0][1] = ( - (a.m[0][1] * a.m[2][2] - a.m[0][2] * a.m[2][1])) / det;
  b.m[0][2] = ( + (a.m[0][1] * a.m[1][2] - a.m[0][2] * a.m[1][1])) / det;
  b.m[1][0] = ( - (a.m[1][0] * a.m[2][2] - a.m[1][2] * a.m[2][0])) / det;
  b.m[1][1] = ( + (a.m[0][0] * a.m[2][2] - a.m[0][2] * a.m[2][0])) / det;
  b.m[1][2] = ( - (a.m[0][0] * a.m[1][2] - a.m[0][2] * a.m[1][0])) / det;
  b.m[2][0] = ( + (a.m[1][0] * a.m[2][1] - a.m[1][1] * a.m[2][0])) / det;
  b.m[2][1] = ( - (a.m[0][0] * a.m[2][1] - a.m[0][1] * a.m[2][0])) / det;
  b.m[2][2] = ( + (a.m[0][0] * a.m[1][1] - a.m[0][1] * a.m[1][0])) / det;
  
  return b;
}


//////////////////////////////////////////////////////////////////////////////
/// Apply a homogeneous transform
GzVector GzHomoMul(GzHomo m, GzVector a)
{
  GzVector b;

  b.x = m.m[0][0] * a.x + m.m[0][1] * a.y + m.m[0][2] * a.z;
  b.y = m.m[1][0] * a.x + m.m[1][1] * a.y + m.m[1][2] * a.z;
  b.z = m.m[2][0] * a.x + m.m[2][1] * a.y + m.m[2][2] * a.z;
  
  return b;
}