vector.cc

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

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/* Desc: Vector classes and functions
 * Author: Andrew Howard
 * Date: 11 Jun 2003
 * CVS: $Id: Vector.cc,v 1.11 2005/11/22 16:44:51 natepak Exp $
 */

#include <assert.h>
#include <stdio.h>

#include "Error.hh"
#include "Vector.hh"


//////////////////////////////////////////////////////////////////////////////
// Create a zero vector
GzVector GzVectorZero()
{
  GzVector v;

  v.x = 0;
  v.y = 0;
  v.z = 0;

  return v;
}


//////////////////////////////////////////////////////////////////////////////
// Create a vector from the given values
GzVector GzVectorSet(double x, double y, double z)
{
  GzVector v;

  v.x = x;
  v.y = y;
  v.z = z;

  return v;
}


//////////////////////////////////////////////////////////////////////////////
// Create a vector from a quaternion
GzVector GzVectorSetQuatern(GzQuatern q)
{
  GzVector v;

  v.x = q.x;
  v.y = q.y;
  v.z = q.z;

  return v;
}


//////////////////////////////////////////////////////////////////////////////
// Add one vector to another (element by element)
// c = b + a
GzVector GzVectorAdd(GzVector b, GzVector a)
{
  return GzVectorSet(b.x + a.x, b.y + a.y, b.z + a.z);
}


//////////////////////////////////////////////////////////////////////////////
// Subtract one vector from another (element by element)
// c = b - a
GzVector GzVectorSub(GzVector b, GzVector a)
{
  return GzVectorSet(b.x - a.x, b.y - a.y, b.z - a.z);
}


//////////////////////////////////////////////////////////////////////////////
// Mutliply vector by scalar
GzVector GzVectorMul(double s, GzVector a)
{
  GzVector b;

  b.x = s * a.x;
  b.y = s * a.y;
  b.z = s * a.z;

  return b;
}


//////////////////////////////////////////////////////////////////////////////
// Compute vector magnitude
double GzVectorMag(GzVector a)
{
  return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
}


//////////////////////////////////////////////////////////////////////////////
// Normalize a vector to unit length
GzVector GzVectorUnit(GzVector a)
{
  double d;
  GzVector b;

  d = sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
  b.x = a.x / d;
  b.y = a.y / d;
  b.z = a.z / d;

  return b;
}


//////////////////////////////////////////////////////////////////////////////
// Take cross product of two vectors
GzVector GzVectorCross(GzVector a, GzVector b)
{
  GzVector c;

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


//////////////////////////////////////////////////////////////////////////////
// Take dot product of two vectors
double GzVectorDot(GzVector a, GzVector b)
{
  return a.x * b.x + a.y * b.y + a.x * b.z;
}


//////////////////////////////////////////////////////////////////////////////
// See if a vector is finite (e.g., not nan)
bool GzVectorFinite(GzVector a)
{
  return finite(a.x) && finite(a.y) && finite(a.z);
}


//////////////////////////////////////////////////////////////////////////////
// Create a quaternion from elements
GzQuatern GzQuaternSet(double u, double x, double y, double z)
{
  GzQuatern q;

  q.u = u;
  q.x = x;
  q.y = y;
  q.z = z;

  return q;
}


//////////////////////////////////////////////////////////////////////////////
// Create a quaternion from a vector
GzQuatern GzQuaternSetVector(GzVector v)
{
  GzQuatern q;

  q.u = 0.0;
  q.x = v.x;
  q.y = v.y;
  q.z = v.z;

  return q;
}


//////////////////////////////////////////////////////////////////////////////
// Create an identity quaternion
GzQuatern GzQuaternIdent()
{
  GzQuatern q;

  q.u = 1.0;
  q.x = 0.0;
  q.y = 0.0;
  q.z = 0.0;
  
  return q;
}


//////////////////////////////////////////////////////////////////////////////
// Create a quaternion from an axis and angle
GzQuatern GzQuaternFromAxis(double x, double y, double z, double a)
{
  double l;
  GzQuatern q;

  l = x * x + y * y + z * z;
  
  if (l > 0.0)
  {
    a *= 0.5;
    l = sin(a) / sqrt(l);
    q.u = cos(a);
    q.x = x * l;
    q.y = y * l;
    q.z = z * l;
  }
  else
  {
    q.u = 1;
    q.x = 0;
    q.y = 0;
    q.z = 0;
  }

  q = GzQuaternNormal(q);
    
  return q;
}


//////////////////////////////////////////////////////////////////////////////
// Convert quaterion to axis and angle
GzQuatern GzQuaternToAxis(GzQuatern a)
{
  GzQuatern b;

  b.x = a.x;
  b.y = a.y;
  b.z = a.z;
  b.u = acos(a.u) * 2;
  
  return b;
}


//////////////////////////////////////////////////////////////////////////////
// Create a quaternion from Euler angles
GzQuatern GzQuaternFromEuler(double roll, double pitch, double yaw)
{
  double phi, the, psi;
  GzQuatern p;

  phi = roll / 2;
  the = pitch / 2;
  psi = yaw / 2;

  // REMOVE
  /*
  c = GzQuaternSet(cos(phi), sin(phi), 0, 0);
  b = GzQuaternSet(cos(the), 0, sin(the), 0);
  a = GzQuaternSet(cos(psi), 0, 0, sin(psi));  
  q = GzQuaternMul(a, GzQuaternMul(b, c));
  */
 
  p.u = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
  p.x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
  p.y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
  //p.z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * sin(psi);
  p.z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);

  p = GzQuaternNormal(p);
  
  return p;
}

//////////////////////////////////////////////////////////////////////////////
// Create a quatern from Homogeneous matrix
GzQuatern GzQuaternFromHomo(GzHomo a)
{
  GzQuatern b;
  b.u = sqrt(1 + a.m[0][0] + a.m[1][1] + a.m[2][2]) / 2;
  assert(b.u >= 1e-16); // TODO: switch cases to avoid instability
  b.x = (a.m[2][1] - a.m[1][2]) / (4 * b.u);
  b.y = (a.m[0][2] - a.m[2][0]) / (4 * b.u);
  b.z = (a.m[1][0] - a.m[0][1]) / (4 * b.u);
  b = GzQuaternNormal(b);
  return b;
}


//////////////////////////////////////////////////////////////////////////////
// Convert quaternion to Euler angles
GzVector GzQuaternToEuler(GzQuatern q)
{
  double phi, the, psi;

  q = GzQuaternNormal(q);
  
  phi = atan2(2 * (q.y*q.z + q.u*q.x), (q.u*q.u - q.x*q.x - q.y*q.y + q.z*q.z));
  the = asin(-2 * (q.x*q.z - q.u * q.y));
  psi = atan2(2 * (q.x*q.y + q.u*q.z), (q.u*q.u + q.x*q.x - q.y*q.y - q.z*q.z));
  
  return GzVectorSet(phi, the, psi);
}


/* REMOVE (not correct)
//////////////////////////////////////////////////////////////////////////////
// Create a Quaternion from a 3x3 rotation matrix
GzQuatern GzQuaternFromRot( double rot[9] )
{
  int which;
  double temp;
  GzQuatern q;

  q.u = (1 + rot[0] + rot[4] + rot[8]) / 4;
  q.x = (1 + rot[0] - rot[4] - rot[8]) / 4;
  q.y = (1 - rot[0] + rot[4] - rot[8]) / 4;
  q.z = (1 - rot[0] - rot[4] + rot[8]) / 4;

  temp = q.u;
  which = 1;
  if (q.x > temp)
  {
    temp = q.x;
    which = 2;
  }
  if (q.y > temp)
  {
    temp = q.y;
    which = 3;
  }
  if (q.z > temp)
  {
    which = 4;
  }

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