Gazebo logo

Vector.hh

Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003  
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /* Desc: Vector classes and functions
00022  * Author: Andrew Howard
00023  * Date: 11 Jun 2003
00024  * CVS: $Id: Vector.hh,v 1.8 2005/11/22 16:44:51 natepak Exp $
00025  */
00026 
00027 #ifndef GZVECTOR_H_
00028 #define GZVECTOR_H_
00029 
00030 #include <math.h>
00031 
00032 #include "Global.hh"
00033 
00034 // Basic 3 vector
00035 struct GzVector
00036 {
00037   double x, y, z;
00038 };
00039 
00040 
00041 // Quatnernion
00042 struct GzQuatern
00043 {
00044   double u, x, y, z;
00045 };
00046 
00047 
00048 // Combined pose
00049 struct GzPose
00050 {
00051   GzVector pos;
00052   GzQuatern rot;
00053 };
00054 
00055 
00057 typedef struct
00058 {
00059   double m[3][3];
00060 } GzHomo;
00061 
00062 
00063 
00064 // Create a zero vector
00065 GzVector GzVectorZero();
00066 
00067 // Create a vector from the given values
00068 GzVector GzVectorSet(double x, double y, double z);
00069 
00070 // Create a vector from a quaternion
00071 GzVector GzVectorSetQuatern(GzQuatern q);
00072 
00073 // Add one vector to another (element by element)
00074 GzVector GzVectorAdd(GzVector b, GzVector a);
00075 
00076 // Subtract one vector from another (element by element)
00077 GzVector GzVectorSub(GzVector b, GzVector a);
00078 
00079 // Mutliply vector by scalar
00080 GzVector GzVectorMul(double s, GzVector a);
00081 
00082 // Compute vector magnitude
00083 double GzVectorMag(GzVector a);
00084 
00085 // Normalize a vector to unit length
00086 GzVector GzVectorUnit(GzVector a);
00087 
00088 // Take cross product of two vectors
00089 GzVector GzVectorCross(GzVector a, GzVector b);
00090 
00091 // Take dot product of two vectors
00092 double GzVectorDot(GzVector a, GzVector b);
00093 
00094 // See if a vector is finite (e.g., not nan)
00095 bool GzVectorFinite(GzVector a);
00096 
00097 
00098 // Create an identity quaternion
00099 GzQuatern GzQuaternIdent();
00100 
00101 // Create a quaternion from elements
00102 GzQuatern GzQuaternSet(double u, double x, double y, double z);
00103 
00104 // Create a quaternion form a vector
00105 GzQuatern GzQuaternSetVector(GzVector v);
00106 
00107 // Invert a quaternion
00108 GzQuatern GzQuaternInverse(GzQuatern a);
00109 
00110 // Normalize a quaternion
00111 GzQuatern GzQuaternNormal(GzQuatern a);
00112 
00113 // Create a quaternion from an axis and angle
00114 GzQuatern GzQuaternFromAxis(double x, double y, double z, double a);
00115 
00116 // Create a quaternion from Euler angles
00117 GzQuatern GzQuaternFromEuler(double roll, double pitch, double yaw);
00118 
00119 // Create a quatern from Homogeneous matrix
00120 GzQuatern GzQuaternFromHomo(GzHomo homo);
00121 
00122 // Convert quaternion to Euler angles (roll, pitch, yaw)
00123 GzVector GzQuaternToEuler(GzQuatern q);
00124 
00125 // Convert quaterion to axis and angle (x, y, z, rotation)
00126 GzQuatern GzQuaternToAxis(GzQuatern a);
00127 
00128 // Multiply two quaternions
00129 GzQuatern GzQuaternMul(GzQuatern a, GzQuatern b);
00130 
00131 // Scale a quaternion rotation
00132 GzQuatern GzQuaternScale(double s, GzQuatern a);
00133 
00134 // See if a quaternion is finite (e.g., not nan)
00135 bool GzQuaternFinite(GzQuatern a);
00136 
00137 
00138 // Create a pose from the given position and rotation
00139 GzPose GzPoseSet(GzVector pos, GzQuatern rot);
00140 
00145 GzPose GzPosePointAt(GzVector pos, GzVector at, GzVector up);
00146 
00147 // See if a pose is finite (e.g., not nan)
00148 bool GzPoseFinite(GzPose a);
00149 
00150 
00151 
00152 
00153 // Add one pose to another: c = b + a
00154 GzPose GzCoordPoseAdd(GzPose bpose, GzPose apose);
00155 
00156 // Subtract one pose from another: c = b - a
00157 GzPose GzCoordPoseSub(GzPose bpose, GzPose apose);
00158 
00159 // Find the inverse of a pose; i.e., if b = ba + a, given b and ba,
00160 // find a
00161 GzPose GzCoordPoseSolve(GzPose ba, GzPose b);
00162 
00163 // Add one position to another: c = b + a
00164 GzVector GzCoordPositionAdd(GzVector bpos, GzVector apos, GzQuatern arot);
00165 
00166 // Subtract one position from another: c = b - a
00167 GzVector GzCoordPositionSub(GzVector bpos, GzVector apos, GzQuatern arot);
00168 
00169 // Add one rotation to another: c = b + a
00170 GzQuatern GzCoordRotationAdd(GzQuatern b, GzQuatern a);
00171 
00172 // Subtract one rotation from another: c = b - a
00173 GzQuatern GzCoordRotationSub(GzQuatern b, GzQuatern a);
00174 
00175 
00177 GzHomo GzHomoFromQuatern(GzQuatern q);
00178 
00180 GzHomo GzHomoInverse(GzHomo a);
00181 
00183 GzVector GzHomoMul(GzHomo m, GzVector a);
00184 
00185 
00186 #endif

Last updated 12 September 2005 21:38:45