Initial commit

tf3-main/include/tf3/LinearMath/Matrix3x3.h

@@ -0,0 +1,696 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef	TF3_MATRIX3x3_H
+#define TF3_MATRIX3x3_H
+
+#include "Vector3.h"
+#include "Quaternion.h"
+
+#include "../macros.h"
+
+namespace tf3
+{
+
+
+#define Matrix3x3Data	Matrix3x3DoubleData 
+
+
+/**@brief The Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with Quaternion, Transform and Vector3.
+* Make sure to only include a pure orthogonal matrix without scaling. */
+class Matrix3x3 {
+
+	///Data storage for the matrix, each vector is a row of the matrix
+	Vector3 m_el[3];
+
+public:
+	/** @brief No initializaion constructor */
+	Matrix3x3 () {}
+
+	//		explicit Matrix3x3(const tf3Scalar *m) { setFromOpenGLSubMatrix(m); }
+
+	/**@brief Constructor from Quaternion */
+	explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
+	/*
+	template <typename tf3Scalar>
+	Matrix3x3(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
+	{ 
+	setEulerYPR(yaw, pitch, roll);
+	}
+	*/
+	/** @brief Constructor with row major formatting */
+	Matrix3x3(const tf3Scalar& xx, const tf3Scalar& xy, const tf3Scalar& xz,
+		const tf3Scalar& yx, const tf3Scalar& yy, const tf3Scalar& yz,
+		const tf3Scalar& zx, const tf3Scalar& zy, const tf3Scalar& zz)
+	{ 
+		setValue(xx, xy, xz, 
+			yx, yy, yz, 
+			zx, zy, zz);
+	}
+	/** @brief Copy constructor */
+	TF3SIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& other)
+	{
+		m_el[0] = other.m_el[0];
+		m_el[1] = other.m_el[1];
+		m_el[2] = other.m_el[2];
+	}
+
+
+	/** @brief Assignment Operator */
+	TF3SIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other)
+	{
+		m_el[0] = other.m_el[0];
+		m_el[1] = other.m_el[1];
+		m_el[2] = other.m_el[2];
+		return *this;
+	}
+
+
+	/** @brief Get a column of the matrix as a vector 
+	*  @param i Column number 0 indexed */
+	TF3SIMD_FORCE_INLINE Vector3 getColumn(int i) const
+	{
+		return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
+	}
+
+
+	/** @brief Get a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	TF3SIMD_FORCE_INLINE const Vector3& getRow(int i) const
+	{
+		tf3FullAssert(0 <= i && i < 3);
+		return m_el[i];
+	}
+
+	/** @brief Get a mutable reference to a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	TF3SIMD_FORCE_INLINE Vector3&  operator[](int i)
+	{ 
+		tf3FullAssert(0 <= i && i < 3);
+		return m_el[i]; 
+	}
+
+	/** @brief Get a const reference to a row of the matrix as a vector 
+	*  @param i Row number 0 indexed */
+	TF3SIMD_FORCE_INLINE const Vector3& operator[](int i) const
+	{
+		tf3FullAssert(0 <= i && i < 3);
+		return m_el[i]; 
+	}
+
+	/** @brief Multiply by the target matrix on the right
+	*  @param m Rotation matrix to be applied 
+	* Equivilant to this = this * m */
+	Matrix3x3& operator*=(const Matrix3x3& m); 
+
+	/** @brief Set from a carray of tf3Scalars 
+	*  @param m A pointer to the beginning of an array of 9 tf3Scalars */
+	void setFromOpenGLSubMatrix(const tf3Scalar *m)
+	{
+		m_el[0].setValue(m[0],m[4],m[8]);
+		m_el[1].setValue(m[1],m[5],m[9]);
+		m_el[2].setValue(m[2],m[6],m[10]);
+
+	}
+	/** @brief Set the values of the matrix explicitly (row major)
+	*  @param xx Top left
+	*  @param xy Top Middle
+	*  @param xz Top Right
+	*  @param yx Middle Left
+	*  @param yy Middle Middle
+	*  @param yz Middle Right
+	*  @param zx Bottom Left
+	*  @param zy Bottom Middle
+	*  @param zz Bottom Right*/
+	void setValue(const tf3Scalar& xx, const tf3Scalar& xy, const tf3Scalar& xz, 
+		const tf3Scalar& yx, const tf3Scalar& yy, const tf3Scalar& yz, 
+		const tf3Scalar& zx, const tf3Scalar& zy, const tf3Scalar& zz)
+	{
+		m_el[0].setValue(xx,xy,xz);
+		m_el[1].setValue(yx,yy,yz);
+		m_el[2].setValue(zx,zy,zz);
+	}
+
+	/** @brief Set the matrix from a quaternion
+	*  @param q The Quaternion to match */  
+	void setRotation(const Quaternion& q) 
+	{
+		tf3Scalar d = q.length2();
+		tf3FullAssert(d != tf3Scalar(0.0));
+		tf3Scalar s = tf3Scalar(2.0) / d;
+		tf3Scalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
+		tf3Scalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
+		tf3Scalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
+		tf3Scalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
+		setValue(tf3Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
+			xy + wz, tf3Scalar(1.0) - (xx + zz), yz - wx,
+			xz - wy, yz + wx, tf3Scalar(1.0) - (xx + yy));
+	}
+
+
+	/** @brief Set the matrix from euler angles using YPR around ZYX respectively
+	*  @param yaw Yaw about Z axis
+	*  @param pitch Pitch about Y axis
+	*  @param roll Roll about X axis 
+	*/
+	ROS_DEPRECATED void setEulerZYX(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
+	{
+		setEulerYPR(yaw, pitch, roll);
+	}
+
+	/** @brief Set the matrix from euler angles YPR around ZYX axes
+	* @param eulerZ Yaw aboud Z axis
+	* @param eulerY Pitch around Y axis
+	* @param eulerX Roll about X axis
+	* 
+	* These angles are used to produce a rotation matrix. The euler
+	* angles are applied in ZYX order. I.e a vector is first rotated 
+	* about X then Y and then Z
+	**/
+	void setEulerYPR(tf3Scalar eulerZ, tf3Scalar eulerY,tf3Scalar eulerX)  { 
+		tf3Scalar ci ( tf3Cos(eulerX)); 
+		tf3Scalar cj ( tf3Cos(eulerY)); 
+		tf3Scalar ch ( tf3Cos(eulerZ)); 
+		tf3Scalar si ( tf3Sin(eulerX)); 
+		tf3Scalar sj ( tf3Sin(eulerY)); 
+		tf3Scalar sh ( tf3Sin(eulerZ)); 
+		tf3Scalar cc = ci * ch; 
+		tf3Scalar cs = ci * sh; 
+		tf3Scalar sc = si * ch; 
+		tf3Scalar ss = si * sh;
+
+		setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+			cj * sh, sj * ss + cc, sj * cs - sc, 
+			-sj,      cj * si,      cj * ci);
+	}
+
+	/** @brief Set the matrix using RPY about XYZ fixed axes
+	 * @param roll Roll about X axis
+         * @param pitch Pitch around Y axis
+         * @param yaw Yaw aboud Z axis
+         * 
+	 **/
+	void setRPY(tf3Scalar roll, tf3Scalar pitch,tf3Scalar yaw) { 
+               setEulerYPR(yaw, pitch, roll);
+	}
+
+	/**@brief Set the matrix to the identity */
+	void setIdentity()
+	{ 
+		setValue(tf3Scalar(1.0), tf3Scalar(0.0), tf3Scalar(0.0), 
+			tf3Scalar(0.0), tf3Scalar(1.0), tf3Scalar(0.0), 
+			tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(1.0)); 
+	}
+
+	static const Matrix3x3&	getIdentity()
+	{
+		static const Matrix3x3 identityMatrix(tf3Scalar(1.0), tf3Scalar(0.0), tf3Scalar(0.0), 
+			tf3Scalar(0.0), tf3Scalar(1.0), tf3Scalar(0.0), 
+			tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(1.0));
+		return identityMatrix;
+	}
+
+	/**@brief Fill the values of the matrix into a 9 element array 
+	* @param m The array to be filled */
+	void getOpenGLSubMatrix(tf3Scalar *m) const 
+	{
+		m[0]  = tf3Scalar(m_el[0].x()); 
+		m[1]  = tf3Scalar(m_el[1].x());
+		m[2]  = tf3Scalar(m_el[2].x());
+		m[3]  = tf3Scalar(0.0); 
+		m[4]  = tf3Scalar(m_el[0].y());
+		m[5]  = tf3Scalar(m_el[1].y());
+		m[6]  = tf3Scalar(m_el[2].y());
+		m[7]  = tf3Scalar(0.0); 
+		m[8]  = tf3Scalar(m_el[0].z()); 
+		m[9]  = tf3Scalar(m_el[1].z());
+		m[10] = tf3Scalar(m_el[2].z());
+		m[11] = tf3Scalar(0.0); 
+	}
+
+	/**@brief Get the matrix represented as a quaternion 
+	* @param q The quaternion which will be set */
+	void getRotation(Quaternion& q) const
+	{
+		tf3Scalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
+		tf3Scalar temp[4];
+
+		if (trace > tf3Scalar(0.0)) 
+		{
+			tf3Scalar s = tf3Sqrt(trace + tf3Scalar(1.0));
+			temp[3]=(s * tf3Scalar(0.5));
+			s = tf3Scalar(0.5) / s;
+
+			temp[0]=((m_el[2].y() - m_el[1].z()) * s);
+			temp[1]=((m_el[0].z() - m_el[2].x()) * s);
+			temp[2]=((m_el[1].x() - m_el[0].y()) * s);
+		} 
+		else 
+		{
+			int i = m_el[0].x() < m_el[1].y() ? 
+				(m_el[1].y() < m_el[2].z() ? 2 : 1) :
+				(m_el[0].x() < m_el[2].z() ? 2 : 0); 
+			int j = (i + 1) % 3;  
+			int k = (i + 2) % 3;
+
+			tf3Scalar s = tf3Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tf3Scalar(1.0));
+			temp[i] = s * tf3Scalar(0.5);
+			s = tf3Scalar(0.5) / s;
+
+			temp[3] = (m_el[k][j] - m_el[j][k]) * s;
+			temp[j] = (m_el[j][i] + m_el[i][j]) * s;
+			temp[k] = (m_el[k][i] + m_el[i][k]) * s;
+		}
+		q.setValue(temp[0],temp[1],temp[2],temp[3]);
+	}
+
+	/**@brief Get the matrix represented as euler angles around ZYX
+	* @param yaw Yaw around Z axis
+	* @param pitch Pitch around Y axis
+	* @param roll around X axis 
+ 	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/	
+	ROS_DEPRECATED void getEulerZYX(tf3Scalar& yaw, tf3Scalar& pitch, tf3Scalar& roll, unsigned int solution_number = 1) const
+	{
+		getEulerYPR(yaw, pitch, roll, solution_number);
+	};
+
+
+	/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
+	* @param yaw Yaw around Z axis
+	* @param pitch Pitch around Y axis
+	* @param roll around X axis */	
+	void getEulerYPR(tf3Scalar& yaw, tf3Scalar& pitch, tf3Scalar& roll, unsigned int solution_number = 1) const
+	{
+		struct Euler
+		{
+			tf3Scalar yaw;
+			tf3Scalar pitch;
+			tf3Scalar roll;
+		};
+
+		Euler euler_out;
+		Euler euler_out2; //second solution
+		//get the pointer to the raw data
+
+		// Check that pitch is not at a singularity
+  		// Check that pitch is not at a singularity
+		if (tf3Fabs(m_el[2].x()) >= 1)
+		{
+			euler_out.yaw = 0;
+			euler_out2.yaw = 0;
+	
+			// From difference of angles formula
+			tf3Scalar delta = tf3Atan2(m_el[2].y(),m_el[2].z());
+			if (m_el[2].x() < 0)  //gimbal locked down
+			{
+				euler_out.pitch = TF3SIMD_PI / tf3Scalar(2.0);
+				euler_out2.pitch = TF3SIMD_PI / tf3Scalar(2.0);
+				euler_out.roll = delta;
+				euler_out2.roll = delta;
+			}
+			else // gimbal locked up
+			{
+				euler_out.pitch = -TF3SIMD_PI / tf3Scalar(2.0);
+				euler_out2.pitch = -TF3SIMD_PI / tf3Scalar(2.0);
+				euler_out.roll = delta;
+				euler_out2.roll = delta;
+			}
+		}
+		else
+		{
+			euler_out.pitch = - tf3Asin(m_el[2].x());
+			euler_out2.pitch = TF3SIMD_PI - euler_out.pitch;
+
+			euler_out.roll = tf3Atan2(m_el[2].y()/tf3Cos(euler_out.pitch), 
+				m_el[2].z()/tf3Cos(euler_out.pitch));
+			euler_out2.roll = tf3Atan2(m_el[2].y()/tf3Cos(euler_out2.pitch), 
+				m_el[2].z()/tf3Cos(euler_out2.pitch));
+
+			euler_out.yaw = tf3Atan2(m_el[1].x()/tf3Cos(euler_out.pitch), 
+				m_el[0].x()/tf3Cos(euler_out.pitch));
+			euler_out2.yaw = tf3Atan2(m_el[1].x()/tf3Cos(euler_out2.pitch), 
+				m_el[0].x()/tf3Cos(euler_out2.pitch));
+		}
+
+		if (solution_number == 1)
+		{ 
+			yaw = euler_out.yaw; 
+			pitch = euler_out.pitch;
+			roll = euler_out.roll;
+		}
+		else
+		{ 
+			yaw = euler_out2.yaw; 
+			pitch = euler_out2.pitch;
+			roll = euler_out2.roll;
+		}
+	}
+
+	/**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ
+	* @param roll around X axis 
+	* @param pitch Pitch around Y axis
+	* @param yaw Yaw around Z axis
+	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/	
+	void getRPY(tf3Scalar& roll, tf3Scalar& pitch, tf3Scalar& yaw, unsigned int solution_number = 1) const
+	{
+	getEulerYPR(yaw, pitch, roll, solution_number);
+	}
+
+	/**@brief Create a scaled copy of the matrix 
+	* @param s Scaling vector The elements of the vector will scale each column */
+
+	Matrix3x3 scaled(const Vector3& s) const
+	{
+		return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
+			m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
+			m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
+	}
+
+	/**@brief Return the determinant of the matrix */
+	tf3Scalar            determinant() const;
+	/**@brief Return the adjoint of the matrix */
+	Matrix3x3 adjoint() const;
+	/**@brief Return the matrix with all values non negative */
+	Matrix3x3 absolute() const;
+	/**@brief Return the transpose of the matrix */
+	Matrix3x3 transpose() const;
+	/**@brief Return the inverse of the matrix */
+	Matrix3x3 inverse() const; 
+
+	Matrix3x3 transposeTimes(const Matrix3x3& m) const;
+	Matrix3x3 timesTranspose(const Matrix3x3& m) const;
+
+	TF3SIMD_FORCE_INLINE tf3Scalar tdotx(const Vector3& v) const 
+	{
+		return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
+	}
+	TF3SIMD_FORCE_INLINE tf3Scalar tdoty(const Vector3& v) const 
+	{
+		return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
+	}
+	TF3SIMD_FORCE_INLINE tf3Scalar tdotz(const Vector3& v) const 
+	{
+		return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
+	}
+
+
+	/**@brief diagonalizes this matrix by the Jacobi method.
+	* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
+	* coordinate system, i.e., old_this = rot * new_this * rot^T. 
+	* @param threshold See iteration
+	* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied 
+	* by the sum of the absolute values of the diagonal, or when maxSteps have been executed. 
+	* 
+	* Note that this matrix is assumed to be symmetric. 
+	*/
+	void diagonalize(Matrix3x3& rot, tf3Scalar threshold, int maxSteps)
+	{
+		rot.setIdentity();
+		for (int step = maxSteps; step > 0; step--)
+		{
+			// find off-diagonal element [p][q] with largest magnitude
+			int p = 0;
+			int q = 1;
+			int r = 2;
+			tf3Scalar max = tf3Fabs(m_el[0][1]);
+			tf3Scalar v = tf3Fabs(m_el[0][2]);
+			if (v > max)
+			{
+				q = 2;
+				r = 1;
+				max = v;
+			}
+			v = tf3Fabs(m_el[1][2]);
+			if (v > max)
+			{
+				p = 1;
+				q = 2;
+				r = 0;
+				max = v;
+			}
+
+			tf3Scalar t = threshold * (tf3Fabs(m_el[0][0]) + tf3Fabs(m_el[1][1]) + tf3Fabs(m_el[2][2]));
+			if (max <= t)
+			{
+				if (max <= TF3SIMD_EPSILON * t)
+				{
+					return;
+				}
+				step = 1;
+			}
+
+			// compute Jacobi rotation J which leads to a zero for element [p][q] 
+			tf3Scalar mpq = m_el[p][q];
+			tf3Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
+			tf3Scalar theta2 = theta * theta;
+			tf3Scalar cos;
+			tf3Scalar sin;
+			if (theta2 * theta2 < tf3Scalar(10 / TF3SIMD_EPSILON))
+			{
+				t = (theta >= 0) ? 1 / (theta + tf3Sqrt(1 + theta2))
+					: 1 / (theta - tf3Sqrt(1 + theta2));
+				cos = 1 / tf3Sqrt(1 + t * t);
+				sin = cos * t;
+			}
+			else
+			{
+				// approximation for large theta-value, i.e., a nearly diagonal matrix
+				t = 1 / (theta * (2 + tf3Scalar(0.5) / theta2));
+				cos = 1 - tf3Scalar(0.5) * t * t;
+				sin = cos * t;
+			}
+
+			// apply rotation to matrix (this = J^T * this * J)
+			m_el[p][q] = m_el[q][p] = 0;
+			m_el[p][p] -= t * mpq;
+			m_el[q][q] += t * mpq;
+			tf3Scalar mrp = m_el[r][p];
+			tf3Scalar mrq = m_el[r][q];
+			m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
+			m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
+
+			// apply rotation to rot (rot = rot * J)
+			for (int i = 0; i < 3; i++)
+			{
+				Vector3& row = rot[i];
+				mrp = row[p];
+				mrq = row[q];
+				row[p] = cos * mrp - sin * mrq;
+				row[q] = cos * mrq + sin * mrp;
+			}
+		}
+	}
+
+
+
+
+	/**@brief Calculate the matrix cofactor 
+	* @param r1 The first row to use for calculating the cofactor
+	* @param c1 The first column to use for calculating the cofactor
+	* @param r1 The second row to use for calculating the cofactor
+	* @param c1 The second column to use for calculating the cofactor
+	* See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
+	*/
+	tf3Scalar cofac(int r1, int c1, int r2, int c2) const 
+	{
+		return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
+	}
+
+	void	serialize(struct	Matrix3x3Data& dataOut) const;
+
+	void	serializeFloat(struct	Matrix3x3FloatData& dataOut) const;
+
+	void	deSerialize(const struct	Matrix3x3Data& dataIn);
+
+	void	deSerializeFloat(const struct	Matrix3x3FloatData& dataIn);
+
+	void	deSerializeDouble(const struct	Matrix3x3DoubleData& dataIn);
+
+};
+
+
+TF3SIMD_FORCE_INLINE Matrix3x3& 
+Matrix3x3::operator*=(const Matrix3x3& m)
+{
+	setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
+		m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
+		m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
+	return *this;
+}
+
+TF3SIMD_FORCE_INLINE tf3Scalar 
+Matrix3x3::determinant() const
+{ 
+	return tf3Triple((*this)[0], (*this)[1], (*this)[2]);
+}
+
+
+TF3SIMD_FORCE_INLINE Matrix3x3 
+Matrix3x3::absolute() const
+{
+	return Matrix3x3(
+		tf3Fabs(m_el[0].x()), tf3Fabs(m_el[0].y()), tf3Fabs(m_el[0].z()),
+		tf3Fabs(m_el[1].x()), tf3Fabs(m_el[1].y()), tf3Fabs(m_el[1].z()),
+		tf3Fabs(m_el[2].x()), tf3Fabs(m_el[2].y()), tf3Fabs(m_el[2].z()));
+}
+
+TF3SIMD_FORCE_INLINE Matrix3x3 
+Matrix3x3::transpose() const 
+{
+	return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
+		m_el[0].y(), m_el[1].y(), m_el[2].y(),
+		m_el[0].z(), m_el[1].z(), m_el[2].z());
+}
+
+TF3SIMD_FORCE_INLINE Matrix3x3 
+Matrix3x3::adjoint() const 
+{
+	return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
+		cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
+		cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
+}
+
+TF3SIMD_FORCE_INLINE Matrix3x3 
+Matrix3x3::inverse() const
+{
+	Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
+	tf3Scalar det = (*this)[0].dot(co);
+	tf3FullAssert(det != tf3Scalar(0.0));
+	tf3Scalar s = tf3Scalar(1.0) / det;
+	return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
+		co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
+		co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
+}
+
+TF3SIMD_FORCE_INLINE Matrix3x3 
+Matrix3x3::transposeTimes(const Matrix3x3& m) const
+{
+	return Matrix3x3(
+		m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
+		m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
+		m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
+		m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
+		m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
+		m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
+		m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
+		m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
+		m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
+}
+
+TF3SIMD_FORCE_INLINE Matrix3x3 
+Matrix3x3::timesTranspose(const Matrix3x3& m) const
+{
+	return Matrix3x3(
+		m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
+		m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
+		m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
+
+}
+
+TF3SIMD_FORCE_INLINE Vector3 
+operator*(const Matrix3x3& m, const Vector3& v) 
+{
+	return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
+}
+
+
+TF3SIMD_FORCE_INLINE Vector3
+operator*(const Vector3& v, const Matrix3x3& m)
+{
+	return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
+}
+
+TF3SIMD_FORCE_INLINE Matrix3x3 
+operator*(const Matrix3x3& m1, const Matrix3x3& m2)
+{
+	return Matrix3x3(
+		m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
+		m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
+		m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
+}
+
+/*
+TF3SIMD_FORCE_INLINE Matrix3x3 tf3MultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) {
+return Matrix3x3(
+m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
+m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
+m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
+m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
+m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
+m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
+m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
+m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
+m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
+}
+*/
+
+/**@brief Equality operator between two matrices
+* It will test all elements are equal.  */
+TF3SIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2)
+{
+	return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
+		m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
+		m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
+}
+
+///for serialization
+struct	Matrix3x3FloatData
+{
+	Vector3FloatData m_el[3];
+};
+
+///for serialization
+struct	Matrix3x3DoubleData
+{
+	Vector3DoubleData m_el[3];
+};
+
+
+	
+
+TF3SIMD_FORCE_INLINE	void	Matrix3x3::serialize(struct	Matrix3x3Data& dataOut) const
+{
+	for (int i=0;i<3;i++)
+		m_el[i].serialize(dataOut.m_el[i]);
+}
+
+TF3SIMD_FORCE_INLINE	void	Matrix3x3::serializeFloat(struct	Matrix3x3FloatData& dataOut) const
+{
+	for (int i=0;i<3;i++)
+		m_el[i].serializeFloat(dataOut.m_el[i]);
+}
+
+
+TF3SIMD_FORCE_INLINE	void	Matrix3x3::deSerialize(const struct	Matrix3x3Data& dataIn)
+{
+	for (int i=0;i<3;i++)
+		m_el[i].deSerialize(dataIn.m_el[i]);
+}
+
+TF3SIMD_FORCE_INLINE	void	Matrix3x3::deSerializeFloat(const struct	Matrix3x3FloatData& dataIn)
+{
+	for (int i=0;i<3;i++)
+		m_el[i].deSerializeFloat(dataIn.m_el[i]);
+}
+
+TF3SIMD_FORCE_INLINE	void	Matrix3x3::deSerializeDouble(const struct	Matrix3x3DoubleData& dataIn)
+{
+	for (int i=0;i<3;i++)
+		m_el[i].deSerializeDouble(dataIn.m_el[i]);
+}
+
+}
+#endif //TF3_MATRIX3x3_H
+

tf3-main/include/tf3/LinearMath/MinMax.h

@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef GEN_MINMAX_H
+#define GEN_MINMAX_H
+
+template <class T>
+TF3SIMD_FORCE_INLINE const T& tf3Min(const T& a, const T& b) 
+{
+  return a < b ? a : b ;
+}
+
+template <class T>
+TF3SIMD_FORCE_INLINE const T& tf3Max(const T& a, const T& b) 
+{
+  return  a > b ? a : b;
+}
+
+template <class T>
+TF3SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) 
+{
+	return a < lb ? lb : (ub < a ? ub : a); 
+}
+
+template <class T>
+TF3SIMD_FORCE_INLINE void tf3SetMin(T& a, const T& b) 
+{
+    if (b < a) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+TF3SIMD_FORCE_INLINE void tf3SetMax(T& a, const T& b) 
+{
+    if (a < b) 
+	{
+		a = b;
+	}
+}
+
+template <class T>
+TF3SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) 
+{
+	if (a < lb) 
+	{
+		a = lb; 
+	}
+	else if (ub < a) 
+	{
+		a = ub;
+	}
+}
+
+#endif

tf3-main/include/tf3/LinearMath/QuadWord.h

@@ -0,0 +1,183 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef TF3SIMD_QUADWORD_H
+#define TF3SIMD_QUADWORD_H
+
+#include "Scalar.h"
+#include "MinMax.h"
+
+
+#if defined (__CELLOS_LV2) && defined (__SPU__)
+#include <altivec.h>
+#endif
+
+namespace tf3
+{
+/**@brief The QuadWord class is base class for Vector3 and Quaternion. 
+ * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
+ */
+#ifndef USE_LIBSPE2
+ATTRIBUTE_ALIGNED16(class) QuadWord
+#else
+class QuadWord
+#endif
+{
+protected:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+	union {
+		vec_float4 mVec128;
+		tf3Scalar	m_floats[4];
+	};
+public:
+	vec_float4	get128() const
+	{
+		return mVec128;
+	}
+protected:
+#else //__CELLOS_LV2__ __SPU__
+	tf3Scalar	m_floats[4];
+#endif //__CELLOS_LV2__ __SPU__
+
+	public:
+  
+
+  /**@brief Return the x value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& getX() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& getY() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& getZ() const { return m_floats[2]; }
+  /**@brief Set the x value */
+		TF3SIMD_FORCE_INLINE void	setX(tf3Scalar x) { m_floats[0] = x;};
+  /**@brief Set the y value */
+		TF3SIMD_FORCE_INLINE void	setY(tf3Scalar y) { m_floats[1] = y;};
+  /**@brief Set the z value */
+		TF3SIMD_FORCE_INLINE void	setZ(tf3Scalar z) { m_floats[2] = z;};
+  /**@brief Set the w value */
+		TF3SIMD_FORCE_INLINE void	setW(tf3Scalar w) { m_floats[3] = w;};
+  /**@brief Return the x value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& x() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& y() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& z() const { return m_floats[2]; }
+  /**@brief Return the w value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& w() const { return m_floats[3]; }
+
+	//TF3SIMD_FORCE_INLINE tf3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}      
+	//TF3SIMD_FORCE_INLINE const tf3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator tf3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	TF3SIMD_FORCE_INLINE	operator       tf3Scalar *()       { return &m_floats[0]; }
+	TF3SIMD_FORCE_INLINE	operator const tf3Scalar *() const { return &m_floats[0]; }
+
+	TF3SIMD_FORCE_INLINE	bool	operator==(const QuadWord& other) const
+	{
+		return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
+	}
+
+	TF3SIMD_FORCE_INLINE	bool	operator!=(const QuadWord& other) const
+	{
+		return !(*this == other);
+	}
+
+  /**@brief Set x,y,z and zero w 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+		TF3SIMD_FORCE_INLINE void 	setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z)
+		{
+			m_floats[0]=x;
+			m_floats[1]=y;
+			m_floats[2]=z;
+			m_floats[3] = 0.f;
+		}
+
+/*		void getValue(tf3Scalar *m) const 
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] = m_floats[2];
+		}
+*/
+/**@brief Set the values 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+		TF3SIMD_FORCE_INLINE void	setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w)
+		{
+			m_floats[0]=x;
+			m_floats[1]=y;
+			m_floats[2]=z;
+			m_floats[3]=w;
+		}
+  /**@brief No initialization constructor */
+		TF3SIMD_FORCE_INLINE QuadWord()
+		//	:m_floats[0](tf3Scalar(0.)),m_floats[1](tf3Scalar(0.)),m_floats[2](tf3Scalar(0.)),m_floats[3](tf3Scalar(0.))
+		{
+		}
+ 
+  /**@brief Three argument constructor (zeros w)
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+		TF3SIMD_FORCE_INLINE QuadWord(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z)		
+		{
+			m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f;
+		}
+
+/**@brief Initializing constructor
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+		TF3SIMD_FORCE_INLINE QuadWord(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w) 
+		{
+			m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w;
+		}
+
+  /**@brief Set each element to the max of the current values and the values of another QuadWord
+   * @param other The other QuadWord to compare with 
+   */
+		TF3SIMD_FORCE_INLINE void	setMax(const QuadWord& other)
+		{
+			tf3SetMax(m_floats[0], other.m_floats[0]);
+			tf3SetMax(m_floats[1], other.m_floats[1]);
+			tf3SetMax(m_floats[2], other.m_floats[2]);
+			tf3SetMax(m_floats[3], other.m_floats[3]);
+		}
+  /**@brief Set each element to the min of the current values and the values of another QuadWord
+   * @param other The other QuadWord to compare with 
+   */
+		TF3SIMD_FORCE_INLINE void	setMin(const QuadWord& other)
+		{
+			tf3SetMin(m_floats[0], other.m_floats[0]);
+			tf3SetMin(m_floats[1], other.m_floats[1]);
+			tf3SetMin(m_floats[2], other.m_floats[2]);
+			tf3SetMin(m_floats[3], other.m_floats[3]);
+		}
+
+
+
+};
+
+}
+#endif //TF3SIMD_QUADWORD_H

tf3-main/include/tf3/LinearMath/Quaternion.h

@@ -0,0 +1,477 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef TF3_QUATERNION_H_
+#define TF3_QUATERNION_H_
+
+
+#include "Vector3.h"
+#include "QuadWord.h"
+
+#include "../macros.h"
+
+namespace tf3
+{
+
+/**@brief The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x3, Vector3 and Transform. */
+class Quaternion : public QuadWord {
+public:
+  /**@brief No initialization constructor */
+	Quaternion() {}
+
+	//		template <typename tf3Scalar>
+	//		explicit Quaternion(const tf3Scalar *v) : Tuple4<tf3Scalar>(v) {}
+  /**@brief Constructor from scalars */
+	Quaternion(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z, const tf3Scalar& w) 
+		: QuadWord(x, y, z, w) 
+	{}
+  /**@brief Axis angle Constructor
+   * @param axis The axis which the rotation is around
+   * @param angle The magnitude of the rotation around the angle (Radians) */
+	Quaternion(const Vector3& axis, const tf3Scalar& angle) 
+	{ 
+		setRotation(axis, angle); 
+	}
+  /**@brief Constructor from Euler angles
+   * @param yaw Angle around Y unless TF3_EULER_DEFAULT_ZYX defined then Z
+   * @param pitch Angle around X unless TF3_EULER_DEFAULT_ZYX defined then Y
+   * @param roll Angle around Z unless TF3_EULER_DEFAULT_ZYX defined then X */
+  ROS_DEPRECATED Quaternion(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
+	{ 
+#ifndef TF3_EULER_DEFAULT_ZYX
+		setEuler(yaw, pitch, roll); 
+#else
+		setRPY(roll, pitch, yaw);
+#endif 
+	}
+  /**@brief Set the rotation using axis angle notation 
+   * @param axis The axis around which to rotate
+   * @param angle The magnitude of the rotation in Radians */
+	void setRotation(const Vector3& axis, const tf3Scalar& angle)
+	{
+		tf3Scalar d = axis.length();
+		tf3Assert(d != tf3Scalar(0.0));
+		tf3Scalar s = tf3Sin(angle * tf3Scalar(0.5)) / d;
+		setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
+			tf3Cos(angle * tf3Scalar(0.5)));
+	}
+  /**@brief Set the quaternion using Euler angles
+   * @param yaw Angle around Y
+   * @param pitch Angle around X
+   * @param roll Angle around Z */
+	void setEuler(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
+	{
+		tf3Scalar halfYaw = tf3Scalar(yaw) * tf3Scalar(0.5);  
+		tf3Scalar halfPitch = tf3Scalar(pitch) * tf3Scalar(0.5);  
+		tf3Scalar halfRoll = tf3Scalar(roll) * tf3Scalar(0.5);  
+		tf3Scalar cosYaw = tf3Cos(halfYaw);
+		tf3Scalar sinYaw = tf3Sin(halfYaw);
+		tf3Scalar cosPitch = tf3Cos(halfPitch);
+		tf3Scalar sinPitch = tf3Sin(halfPitch);
+		tf3Scalar cosRoll = tf3Cos(halfRoll);
+		tf3Scalar sinRoll = tf3Sin(halfRoll);
+		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
+			cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
+			sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
+			cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
+	}
+  /**@brief Set the quaternion using fixed axis RPY
+   * @param roll Angle around X 
+   * @param pitch Angle around Y
+   * @param yaw Angle around Z*/
+  void setRPY(const tf3Scalar& roll, const tf3Scalar& pitch, const tf3Scalar& yaw)
+	{
+		tf3Scalar halfYaw = tf3Scalar(yaw) * tf3Scalar(0.5);  
+		tf3Scalar halfPitch = tf3Scalar(pitch) * tf3Scalar(0.5);  
+		tf3Scalar halfRoll = tf3Scalar(roll) * tf3Scalar(0.5);  
+		tf3Scalar cosYaw = tf3Cos(halfYaw);
+		tf3Scalar sinYaw = tf3Sin(halfYaw);
+		tf3Scalar cosPitch = tf3Cos(halfPitch);
+		tf3Scalar sinPitch = tf3Sin(halfPitch);
+		tf3Scalar cosRoll = tf3Cos(halfRoll);
+		tf3Scalar sinRoll = tf3Sin(halfRoll);
+		setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
+                         cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
+                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
+                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
+	}
+  /**@brief Set the quaternion using euler angles 
+   * @param yaw Angle around Z
+   * @param pitch Angle around Y
+   * @param roll Angle around X */
+  ROS_DEPRECATED void setEulerZYX(const tf3Scalar& yaw, const tf3Scalar& pitch, const tf3Scalar& roll)
+	{
+          setRPY(roll, pitch, yaw);
+	}
+  /**@brief Add two quaternions
+   * @param q The quaternion to add to this one */
+	TF3SIMD_FORCE_INLINE	Quaternion& operator+=(const Quaternion& q)
+	{
+		m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
+		return *this;
+	}
+
+  /**@brief Sutf3ract out a quaternion
+   * @param q The quaternion to sutf3ract from this one */
+	Quaternion& operator-=(const Quaternion& q) 
+	{
+		m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
+		return *this;
+	}
+
+  /**@brief Scale this quaternion
+   * @param s The scalar to scale by */
+	Quaternion& operator*=(const tf3Scalar& s)
+	{
+		m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
+		return *this;
+	}
+
+  /**@brief Multiply this quaternion by q on the right
+   * @param q The other quaternion 
+   * Equivilant to this = this * q */
+	Quaternion& operator*=(const Quaternion& q)
+	{
+		setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
+			m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
+			m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
+			m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
+		return *this;
+	}
+  /**@brief Return the dot product between this quaternion and another
+   * @param q The other quaternion */
+	tf3Scalar dot(const Quaternion& q) const
+	{
+		return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
+	}
+
+  /**@brief Return the length squared of the quaternion */
+	tf3Scalar length2() const
+	{
+		return dot(*this);
+	}
+
+  /**@brief Return the length of the quaternion */
+	tf3Scalar length() const
+	{
+		return tf3Sqrt(length2());
+	}
+
+  /**@brief Normalize the quaternion 
+   * Such that x^2 + y^2 + z^2 +w^2 = 1 */
+	Quaternion& normalize() 
+	{
+		return *this /= length();
+	}
+
+  /**@brief Return a scaled version of this quaternion
+   * @param s The scale factor */
+	TF3SIMD_FORCE_INLINE Quaternion
+	operator*(const tf3Scalar& s) const
+	{
+		return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
+	}
+
+
+  /**@brief Return an inversely scaled versionof this quaternion
+   * @param s The inverse scale factor */
+	Quaternion operator/(const tf3Scalar& s) const
+	{
+		tf3Assert(s != tf3Scalar(0.0));
+		return *this * (tf3Scalar(1.0) / s);
+	}
+
+  /**@brief Inversely scale this quaternion
+   * @param s The scale factor */
+	Quaternion& operator/=(const tf3Scalar& s) 
+	{
+		tf3Assert(s != tf3Scalar(0.0));
+		return *this *= tf3Scalar(1.0) / s;
+	}
+
+  /**@brief Return a normalized version of this quaternion */
+	Quaternion normalized() const 
+	{
+		return *this / length();
+	} 
+  /**@brief Return the ***half*** angle between this quaternion and the other 
+   * @param q The other quaternion */
+	tf3Scalar angle(const Quaternion& q) const 
+	{
+		tf3Scalar s = tf3Sqrt(length2() * q.length2());
+		tf3Assert(s != tf3Scalar(0.0));
+		return tf3Acos(dot(q) / s);
+	}
+	/**@brief Return the angle between this quaternion and the other along the shortest path
+	* @param q The other quaternion */
+	tf3Scalar angleShortestPath(const Quaternion& q) const 
+	{
+		tf3Scalar s = tf3Sqrt(length2() * q.length2());
+		tf3Assert(s != tf3Scalar(0.0));
+		if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+			return tf3Acos(dot(-q) / s) * tf3Scalar(2.0);
+		else 
+			return tf3Acos(dot(q) / s) * tf3Scalar(2.0);
+	}
+        /**@brief Return the angle [0, 2Pi] of rotation represented by this quaternion */
+	tf3Scalar getAngle() const 
+	{
+		tf3Scalar s = tf3Scalar(2.) * tf3Acos(m_floats[3]);
+		return s;
+	}
+
+        /**@brief Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path */
+	tf3Scalar getAngleShortestPath() const 
+	{
+	tf3Scalar s;
+		if (m_floats[3] >= 0)
+			s = tf3Scalar(2.) * tf3Acos(m_floats[3]);
+		else
+			s = tf3Scalar(2.) * tf3Acos(-m_floats[3]);
+
+		return s;
+	}
+
+	/**@brief Return the axis of the rotation represented by this quaternion */
+	Vector3 getAxis() const
+	{
+		tf3Scalar s_squared = tf3Scalar(1.) - tf3Pow(m_floats[3], tf3Scalar(2.));
+		if (s_squared < tf3Scalar(10.) * TF3SIMD_EPSILON) //Check for divide by zero
+			return Vector3(1.0, 0.0, 0.0);  // Arbitrary
+		tf3Scalar s = tf3Sqrt(s_squared);
+		return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
+	}
+
+	/**@brief Return the inverse of this quaternion */
+	Quaternion inverse() const
+	{
+		return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
+	}
+
+  /**@brief Return the sum of this quaternion and the other 
+   * @param q2 The other quaternion */
+	TF3SIMD_FORCE_INLINE Quaternion
+	operator+(const Quaternion& q2) const
+	{
+		const Quaternion& q1 = *this;
+		return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
+	}
+
+  /**@brief Return the difference between this quaternion and the other 
+   * @param q2 The other quaternion */
+	TF3SIMD_FORCE_INLINE Quaternion
+	operator-(const Quaternion& q2) const
+	{
+		const Quaternion& q1 = *this;
+		return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
+	}
+
+  /**@brief Return the negative of this quaternion 
+   * This simply negates each element */
+	TF3SIMD_FORCE_INLINE Quaternion operator-() const
+	{
+		const Quaternion& q2 = *this;
+		return Quaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
+	}
+  /**@todo document this and it's use */
+	TF3SIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const 
+	{
+		Quaternion diff,sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if( diff.dot(diff) > sum.dot(sum) )
+			return qd;
+		return (-qd);
+	}
+
+	/**@todo document this and it's use */
+	TF3SIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const 
+	{
+		Quaternion diff,sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if( diff.dot(diff) < sum.dot(sum) )
+			return qd;
+		return (-qd);
+	}
+
+
+  /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
+   * @param q The other quaternion to interpolate with 
+   * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
+   * Slerp interpolates assuming constant velocity.  */
+	Quaternion slerp(const Quaternion& q, const tf3Scalar& t) const
+	{
+          tf3Scalar theta = angleShortestPath(q) / tf3Scalar(2.0);
+		if (theta != tf3Scalar(0.0))
+		{
+			tf3Scalar d = tf3Scalar(1.0) / tf3Sin(theta);
+			tf3Scalar s0 = tf3Sin((tf3Scalar(1.0) - t) * theta);
+			tf3Scalar s1 = tf3Sin(t * theta);   
+                        if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+                          return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
+                                              (m_floats[1] * s0 + -q.y() * s1) * d,
+                                              (m_floats[2] * s0 + -q.z() * s1) * d,
+                                              (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
+                        else
+                          return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
+                                              (m_floats[1] * s0 + q.y() * s1) * d,
+                                              (m_floats[2] * s0 + q.z() * s1) * d,
+                                              (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+                        
+		}
+		else
+		{
+			return *this;
+		}
+	}
+
+	static const Quaternion&	getIdentity()
+	{
+		static const Quaternion identityQuat(tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(1.));
+		return identityQuat;
+	}
+
+	TF3SIMD_FORCE_INLINE const tf3Scalar& getW() const { return m_floats[3]; }
+
+	
+};
+
+
+/**@brief Return the negative of a quaternion */
+TF3SIMD_FORCE_INLINE Quaternion
+operator-(const Quaternion& q)
+{
+	return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
+}
+
+
+
+/**@brief Return the product of two quaternions */
+TF3SIMD_FORCE_INLINE Quaternion
+operator*(const Quaternion& q1, const Quaternion& q2) {
+	return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
+		q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
+		q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
+		q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
+}
+
+TF3SIMD_FORCE_INLINE Quaternion
+operator*(const Quaternion& q, const Vector3& w)
+{
+	return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
+		q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
+		q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
+		-q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
+}
+
+TF3SIMD_FORCE_INLINE Quaternion
+operator*(const Vector3& w, const Quaternion& q)
+{
+	return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
+		w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
+		w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
+		-w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
+}
+
+/**@brief Calculate the dot product between two quaternions */
+TF3SIMD_FORCE_INLINE tf3Scalar 
+dot(const Quaternion& q1, const Quaternion& q2) 
+{ 
+	return q1.dot(q2); 
+}
+
+
+/**@brief Return the length of a quaternion */
+TF3SIMD_FORCE_INLINE tf3Scalar
+length(const Quaternion& q) 
+{ 
+	return q.length(); 
+}
+
+/**@brief Return the ***half*** angle between two quaternions*/
+TF3SIMD_FORCE_INLINE tf3Scalar
+angle(const Quaternion& q1, const Quaternion& q2) 
+{ 
+	return q1.angle(q2); 
+}
+
+/**@brief Return the shortest angle between two quaternions*/
+TF3SIMD_FORCE_INLINE tf3Scalar
+angleShortestPath(const Quaternion& q1, const Quaternion& q2) 
+{ 
+	return q1.angleShortestPath(q2); 
+}
+
+/**@brief Return the inverse of a quaternion*/
+TF3SIMD_FORCE_INLINE Quaternion
+inverse(const Quaternion& q) 
+{
+	return q.inverse();
+}
+
+/**@brief Return the result of spherical linear interpolation betwen two quaternions 
+ * @param q1 The first quaternion
+ * @param q2 The second quaternion 
+ * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2 
+ * Slerp assumes constant velocity between positions. */
+TF3SIMD_FORCE_INLINE Quaternion
+slerp(const Quaternion& q1, const Quaternion& q2, const tf3Scalar& t) 
+{
+	return q1.slerp(q2, t);
+}
+
+TF3SIMD_FORCE_INLINE Vector3 
+quatRotate(const Quaternion& rotation, const Vector3& v) 
+{
+	Quaternion q = rotation * v;
+	q *= rotation.inverse();
+	return Vector3(q.getX(),q.getY(),q.getZ());
+}
+
+TF3SIMD_FORCE_INLINE Quaternion 
+shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
+{
+	Vector3 c = v0.cross(v1);
+	tf3Scalar  d = v0.dot(v1);
+
+	if (d < -1.0 + TF3SIMD_EPSILON)
+	{
+		Vector3 n,unused;
+		tf3PlaneSpace1(v0,n,unused);
+		return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
+	}
+
+	tf3Scalar  s = tf3Sqrt((1.0f + d) * 2.0f);
+	tf3Scalar rs = 1.0f / s;
+
+	return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
+}
+
+TF3SIMD_FORCE_INLINE Quaternion 
+shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
+{
+	v0.normalize();
+	v1.normalize();
+	return shortestArcQuat(v0,v1);
+}
+
+}
+#endif
+
+
+
+

tf3-main/include/tf3/LinearMath/Scalar.h

@@ -0,0 +1,417 @@
+/*
+Copyright (c) 2003-2009 Erwin Coumans  http://bullet.googlecode.com
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef TF3_SCALAR_H
+#define TF3_SCALAR_H
+
+#ifdef TF3_MANAGED_CODE
+//Aligned data types not supported in managed code
+#pragma unmanaged
+#endif
+
+
+#include <math.h>
+#include <stdlib.h>//size_t for MSVC 6.0
+#include <cstdlib>
+#include <cfloat>
+#include <float.h>
+
+#if defined(DEBUG) || defined (_DEBUG)
+#define TF3_DEBUG
+#endif
+
+
+#ifdef _WIN32
+
+		#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300)
+
+			#define TF3SIMD_FORCE_INLINE inline
+			#define ATTRIBUTE_ALIGNED16(a) a
+			#define ATTRIBUTE_ALIGNED64(a) a
+			#define ATTRIBUTE_ALIGNED128(a) a
+		#else
+			//#define TF3_HAS_ALIGNED_ALLOCATOR
+			#pragma warning(disable : 4324) // disable padding warning
+//			#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
+//			#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
+//			#pragma warning(disable:4786) // Disable the "debug name too long" warning
+
+			#define TF3SIMD_FORCE_INLINE __forceinline
+			#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
+			#define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
+			#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
+		#ifdef _XBOX
+			#define TF3_USE_VMX128
+
+			#include <ppcintrinsics.h>
+ 			#define TF3_HAVE_NATIVE_FSEL
+ 			#define tf3Fsel(a,b,c) __fsel((a),(b),(c))
+		#else
+
+
+		#endif//_XBOX
+
+		#endif //__MINGW32__
+
+		#include <assert.h>
+#ifdef TF3_DEBUG
+		#define tf3Assert assert
+#else
+		#define tf3Assert(x)
+#endif
+		//tf3FullAssert is optional, slows down a lot
+		#define tf3FullAssert(x)
+
+		#define tf3Likely(_c)  _c
+		#define tf3Unlikely(_c) _c
+
+#else
+	
+#if defined	(__CELLOS_LV2__)
+		#define TF3SIMD_FORCE_INLINE inline
+		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+		#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+		#ifndef assert
+		#include <assert.h>
+		#endif
+#ifdef TF3_DEBUG
+		#define tf3Assert assert
+#else
+		#define tf3Assert(x)
+#endif
+		//tf3FullAssert is optional, slows down a lot
+		#define tf3FullAssert(x)
+
+		#define tf3Likely(_c)  _c
+		#define tf3Unlikely(_c) _c
+
+#else
+
+#ifdef USE_LIBSPE2
+
+		#define TF3SIMD_FORCE_INLINE __inline
+		#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+		#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+		#ifndef assert
+		#include <assert.h>
+		#endif
+#ifdef TF3_DEBUG
+		#define tf3Assert assert
+#else
+		#define tf3Assert(x)
+#endif
+		//tf3FullAssert is optional, slows down a lot
+		#define tf3FullAssert(x)
+
+
+		#define tf3Likely(_c)   __builtin_expect((_c), 1)
+		#define tf3Unlikely(_c) __builtin_expect((_c), 0)
+		
+
+#else
+	//non-windows systems
+
+		#define TF3SIMD_FORCE_INLINE inline
+		///@todo: check out alignment methods for other platforms/compilers
+		///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+		///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
+		///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
+		#define ATTRIBUTE_ALIGNED16(a) a
+		#define ATTRIBUTE_ALIGNED64(a) a
+		#define ATTRIBUTE_ALIGNED128(a) a
+		#ifndef assert
+		#include <assert.h>
+		#endif
+
+#if defined(DEBUG) || defined (_DEBUG)
+		#define tf3Assert assert
+#else
+		#define tf3Assert(x)
+#endif
+
+		//tf3FullAssert is optional, slows down a lot
+		#define tf3FullAssert(x)
+		#define tf3Likely(_c)  _c
+		#define tf3Unlikely(_c) _c
+
+#endif // LIBSPE2
+
+#endif	//__CELLOS_LV2__
+#endif
+
+
+///The tf3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
+typedef double tf3Scalar;
+//this number could be bigger in double precision
+#define TF3_LARGE_FLOAT 1e30
+
+
+#define TF3_DECLARE_ALIGNED_ALLOCATOR() \
+   TF3SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes)   { return tf3AlignedAlloc(sizeInBytes,16); }   \
+   TF3SIMD_FORCE_INLINE void  operator delete(void* ptr)         { tf3AlignedFree(ptr); }   \
+   TF3SIMD_FORCE_INLINE void* operator new(size_t, void* ptr)   { return ptr; }   \
+   TF3SIMD_FORCE_INLINE void  operator delete(void*, void*)      { }   \
+   TF3SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes)   { return tf3AlignedAlloc(sizeInBytes,16); }   \
+   TF3SIMD_FORCE_INLINE void  operator delete[](void* ptr)         { tf3AlignedFree(ptr); }   \
+   TF3SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr)   { return ptr; }   \
+   TF3SIMD_FORCE_INLINE void  operator delete[](void*, void*)      { }   \
+
+
+
+		
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Sqrt(tf3Scalar x) { return sqrt(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Fabs(tf3Scalar x) { return fabs(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Cos(tf3Scalar x) { return cos(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Sin(tf3Scalar x) { return sin(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Tan(tf3Scalar x) { return tan(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Acos(tf3Scalar x) { if (x<tf3Scalar(-1))	x=tf3Scalar(-1); if (x>tf3Scalar(1))	x=tf3Scalar(1); return acos(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Asin(tf3Scalar x) { if (x<tf3Scalar(-1))	x=tf3Scalar(-1); if (x>tf3Scalar(1))	x=tf3Scalar(1); return asin(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan(tf3Scalar x) { return atan(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan2(tf3Scalar x, tf3Scalar y) { return atan2(x, y); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Exp(tf3Scalar x) { return exp(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Log(tf3Scalar x) { return log(x); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Pow(tf3Scalar x,tf3Scalar y) { return pow(x,y); }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Fmod(tf3Scalar x,tf3Scalar y) { return fmod(x,y); }
+
+
+#define TF3SIMD_2_PI         tf3Scalar(6.283185307179586232)
+#define TF3SIMD_PI           (TF3SIMD_2_PI * tf3Scalar(0.5))
+#define TF3SIMD_HALF_PI      (TF3SIMD_2_PI * tf3Scalar(0.25))
+#define TF3SIMD_RADS_PER_DEG (TF3SIMD_2_PI / tf3Scalar(360.0))
+#define TF3SIMD_DEGS_PER_RAD  (tf3Scalar(360.0) / TF3SIMD_2_PI)
+#define TF3SIMDSQRT12 tf3Scalar(0.7071067811865475244008443621048490)
+
+#define tf3RecipSqrt(x) ((tf3Scalar)(tf3Scalar(1.0)/tf3Sqrt(tf3Scalar(x))))		/* reciprocal square root */
+
+
+#define TF3SIMD_EPSILON      DBL_EPSILON
+#define TF3SIMD_INFINITY     DBL_MAX
+
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Atan2Fast(tf3Scalar y, tf3Scalar x) 
+{
+	tf3Scalar coeff_1 = TF3SIMD_PI / 4.0f;
+	tf3Scalar coeff_2 = 3.0f * coeff_1;
+	tf3Scalar abs_y = tf3Fabs(y);
+	tf3Scalar angle;
+	if (x >= 0.0f) {
+		tf3Scalar r = (x - abs_y) / (x + abs_y);
+		angle = coeff_1 - coeff_1 * r;
+	} else {
+		tf3Scalar r = (x + abs_y) / (abs_y - x);
+		angle = coeff_2 - coeff_1 * r;
+	}
+	return (y < 0.0f) ? -angle : angle;
+}
+
+TF3SIMD_FORCE_INLINE bool      tf3FuzzyZero(tf3Scalar x) { return tf3Fabs(x) < TF3SIMD_EPSILON; }
+
+TF3SIMD_FORCE_INLINE bool	tf3Equal(tf3Scalar a, tf3Scalar eps) {
+	return (((a) <= eps) && !((a) < -eps));
+}
+TF3SIMD_FORCE_INLINE bool	tf3GreaterEqual (tf3Scalar a, tf3Scalar eps) {
+	return (!((a) <= eps));
+}
+
+
+TF3SIMD_FORCE_INLINE int       tf3IsNegative(tf3Scalar x) {
+    return x < tf3Scalar(0.0) ? 1 : 0;
+}
+
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Radians(tf3Scalar x) { return x * TF3SIMD_RADS_PER_DEG; }
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Degrees(tf3Scalar x) { return x * TF3SIMD_DEGS_PER_RAD; }
+
+#define TF3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+
+#ifndef tf3Fsel
+TF3SIMD_FORCE_INLINE tf3Scalar tf3Fsel(tf3Scalar a, tf3Scalar b, tf3Scalar c)
+{
+	return a >= 0 ? b : c;
+}
+#endif
+#define tf3Fsels(a,b,c) (tf3Scalar)tf3Fsel(a,b,c)
+
+
+TF3SIMD_FORCE_INLINE bool tf3MachineIsLittleEndian()
+{
+   long int i = 1;
+   const char *p = (const char *) &i;
+   if (p[0] == 1)  // Lowest address contains the least significant byte
+	   return true;
+   else
+	   return false;
+}
+
+
+
+///tf3Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
+///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
+TF3SIMD_FORCE_INLINE unsigned tf3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) 
+{
+    // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
+    // Rely on positive value or'ed with its negative having sign bit on
+    // and zero value or'ed with its negative (which is still zero) having sign bit off 
+    // Use arithmetic shift right, shifting the sign bit through all 32 bits
+    unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+    unsigned testEqz = ~testNz;
+    return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); 
+}
+TF3SIMD_FORCE_INLINE int tf3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
+{
+    unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
+    unsigned testEqz = ~testNz; 
+    return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
+}
+TF3SIMD_FORCE_INLINE float tf3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
+{
+#ifdef TF3_HAVE_NATIVE_FSEL
+    return (float)tf3Fsel((tf3Scalar)condition - tf3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
+#else
+    return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; 
+#endif
+}
+
+template<typename T> TF3SIMD_FORCE_INLINE void tf3Swap(T& a, T& b)
+{
+	T tmp = a;
+	a = b;
+	b = tmp;
+}
+
+
+//PCK: endian swapping functions
+TF3SIMD_FORCE_INLINE unsigned tf3SwapEndian(unsigned val)
+{
+	return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8)  | ((val & 0x000000ff) << 24));
+}
+
+TF3SIMD_FORCE_INLINE unsigned short tf3SwapEndian(unsigned short val)
+{
+	return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
+}
+
+TF3SIMD_FORCE_INLINE unsigned tf3SwapEndian(int val)
+{
+	return tf3SwapEndian((unsigned)val);
+}
+
+TF3SIMD_FORCE_INLINE unsigned short tf3SwapEndian(short val)
+{
+	return tf3SwapEndian((unsigned short) val);
+}
+
+///tf3SwapFloat uses using char pointers to swap the endianness
+////tf3SwapFloat/tf3SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
+///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754. 
+///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception. 
+///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you. 
+///so instead of returning a float/double, we return integer/long long integer
+TF3SIMD_FORCE_INLINE unsigned int  tf3SwapEndianFloat(float d)
+{
+    unsigned int a = 0;
+    unsigned char *dst = (unsigned char *)&a;
+    unsigned char *src = (unsigned char *)&d;
+
+    dst[0] = src[3];
+    dst[1] = src[2];
+    dst[2] = src[1];
+    dst[3] = src[0];
+    return a;
+}
+
+// unswap using char pointers
+TF3SIMD_FORCE_INLINE float tf3UnswapEndianFloat(unsigned int a) 
+{
+    float d = 0.0f;
+    unsigned char *src = (unsigned char *)&a;
+    unsigned char *dst = (unsigned char *)&d;
+
+    dst[0] = src[3];
+    dst[1] = src[2];
+    dst[2] = src[1];
+    dst[3] = src[0];
+
+    return d;
+}
+
+
+// swap using char pointers
+TF3SIMD_FORCE_INLINE void  tf3SwapEndianDouble(double d, unsigned char* dst)
+{
+    unsigned char *src = (unsigned char *)&d;
+
+    dst[0] = src[7];
+    dst[1] = src[6];
+    dst[2] = src[5];
+    dst[3] = src[4];
+    dst[4] = src[3];
+    dst[5] = src[2];
+    dst[6] = src[1];
+    dst[7] = src[0];
+
+}
+
+// unswap using char pointers
+TF3SIMD_FORCE_INLINE double tf3UnswapEndianDouble(const unsigned char *src) 
+{
+    double d = 0.0;
+    unsigned char *dst = (unsigned char *)&d;
+
+    dst[0] = src[7];
+    dst[1] = src[6];
+    dst[2] = src[5];
+    dst[3] = src[4];
+    dst[4] = src[3];
+    dst[5] = src[2];
+    dst[6] = src[1];
+    dst[7] = src[0];
+
+	return d;
+}
+
+// returns normalized value in range [-TF3SIMD_PI, TF3SIMD_PI]
+TF3SIMD_FORCE_INLINE tf3Scalar tf3NormalizeAngle(tf3Scalar angleInRadians) 
+{
+	angleInRadians = tf3Fmod(angleInRadians, TF3SIMD_2_PI);
+	if(angleInRadians < -TF3SIMD_PI)
+	{
+		return angleInRadians + TF3SIMD_2_PI;
+	}
+	else if(angleInRadians > TF3SIMD_PI)
+	{
+		return angleInRadians - TF3SIMD_2_PI;
+	}
+	else
+	{
+		return angleInRadians;
+	}
+}
+
+///rudimentary class to provide type info
+struct tf3TypedObject
+{
+	tf3TypedObject(int objectType)
+		:m_objectType(objectType)
+	{
+	}
+	int	m_objectType;
+	inline int getObjectType() const
+	{
+		return m_objectType;
+	}
+};
+#endif //TF3SIMD___SCALAR_H

tf3-main/include/tf3/LinearMath/Transform.h

@@ -0,0 +1,305 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef tf3_Transform_H
+#define tf3_Transform_H
+
+
+#include "Matrix3x3.h"
+
+
+namespace tf3
+{
+
+#define TransformData TransformDoubleData
+
+
+/**@brief The Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
+ *It can be used in combination with Vector3, Quaternion and Matrix3x3 linear algebra classes. */
+class Transform {
+	
+  ///Storage for the rotation
+	Matrix3x3 m_basis;
+  ///Storage for the translation
+	Vector3   m_origin;
+
+public:
+	
+  /**@brief No initialization constructor */
+	Transform() {}
+  /**@brief Constructor from Quaternion (optional Vector3 )
+   * @param q Rotation from quaternion 
+   * @param c Translation from Vector (default 0,0,0) */
+	explicit TF3SIMD_FORCE_INLINE Transform(const Quaternion& q, 
+		const Vector3& c = Vector3(tf3Scalar(0), tf3Scalar(0), tf3Scalar(0))) 
+		: m_basis(q),
+		m_origin(c)
+	{}
+
+  /**@brief Constructor from Matrix3x3 (optional Vector3)
+   * @param b Rotation from Matrix 
+   * @param c Translation from Vector default (0,0,0)*/
+	explicit TF3SIMD_FORCE_INLINE Transform(const Matrix3x3& b, 
+		const Vector3& c = Vector3(tf3Scalar(0), tf3Scalar(0), tf3Scalar(0)))
+		: m_basis(b),
+		m_origin(c)
+	{}
+  /**@brief Copy constructor */
+	TF3SIMD_FORCE_INLINE Transform (const Transform& other)
+		: m_basis(other.m_basis),
+		m_origin(other.m_origin)
+	{
+	}
+  /**@brief Assignment Operator */
+	TF3SIMD_FORCE_INLINE Transform& operator=(const Transform& other)
+	{
+		m_basis = other.m_basis;
+		m_origin = other.m_origin;
+		return *this;
+	}
+
+  /**@brief Set the current transform as the value of the product of two transforms
+   * @param t1 Transform 1
+   * @param t2 Transform 2
+   * This = Transform1 * Transform2 */
+		TF3SIMD_FORCE_INLINE void mult(const Transform& t1, const Transform& t2) {
+			m_basis = t1.m_basis * t2.m_basis;
+			m_origin = t1(t2.m_origin);
+		}
+
+/*		void multInverseLeft(const Transform& t1, const Transform& t2) {
+			Vector3 v = t2.m_origin - t1.m_origin;
+			m_basis = tf3MultTransposeLeft(t1.m_basis, t2.m_basis);
+			m_origin = v * t1.m_basis;
+		}
+		*/
+
+/**@brief Return the transform of the vector */
+	TF3SIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const
+	{
+		return Vector3(m_basis[0].dot(x) + m_origin.x(), 
+			m_basis[1].dot(x) + m_origin.y(), 
+			m_basis[2].dot(x) + m_origin.z());
+	}
+
+  /**@brief Return the transform of the vector */
+	TF3SIMD_FORCE_INLINE Vector3 operator*(const Vector3& x) const
+	{
+		return (*this)(x);
+	}
+
+  /**@brief Return the transform of the Quaternion */
+	TF3SIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q) const
+	{
+		return getRotation() * q;
+	}
+
+  /**@brief Return the basis matrix for the rotation */
+	TF3SIMD_FORCE_INLINE Matrix3x3&       getBasis()          { return m_basis; }
+  /**@brief Return the basis matrix for the rotation */
+	TF3SIMD_FORCE_INLINE const Matrix3x3& getBasis()    const { return m_basis; }
+
+  /**@brief Return the origin vector translation */
+	TF3SIMD_FORCE_INLINE Vector3&         getOrigin()         { return m_origin; }
+  /**@brief Return the origin vector translation */
+	TF3SIMD_FORCE_INLINE const Vector3&   getOrigin()   const { return m_origin; }
+
+  /**@brief Return a quaternion representing the rotation */
+	Quaternion getRotation() const { 
+		Quaternion q;
+		m_basis.getRotation(q);
+		return q;
+	}
+	
+	
+  /**@brief Set from an array 
+   * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
+	void setFromOpenGLMatrix(const tf3Scalar *m)
+	{
+		m_basis.setFromOpenGLSubMatrix(m);
+		m_origin.setValue(m[12],m[13],m[14]);
+	}
+
+  /**@brief Fill an array representation
+   * @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
+	void getOpenGLMatrix(tf3Scalar *m) const 
+	{
+		m_basis.getOpenGLSubMatrix(m);
+		m[12] = m_origin.x();
+		m[13] = m_origin.y();
+		m[14] = m_origin.z();
+		m[15] = tf3Scalar(1.0);
+	}
+
+  /**@brief Set the translational element
+   * @param origin The vector to set the translation to */
+	TF3SIMD_FORCE_INLINE void setOrigin(const Vector3& origin) 
+	{ 
+		m_origin = origin;
+	}
+
+	TF3SIMD_FORCE_INLINE Vector3 invXform(const Vector3& inVec) const;
+
+
+  /**@brief Set the rotational element by Matrix3x3 */
+	TF3SIMD_FORCE_INLINE void setBasis(const Matrix3x3& basis)
+	{ 
+		m_basis = basis;
+	}
+
+  /**@brief Set the rotational element by Quaternion */
+	TF3SIMD_FORCE_INLINE void setRotation(const Quaternion& q)
+	{
+		m_basis.setRotation(q);
+	}
+
+
+  /**@brief Set this transformation to the identity */
+	void setIdentity()
+	{
+		m_basis.setIdentity();
+		m_origin.setValue(tf3Scalar(0.0), tf3Scalar(0.0), tf3Scalar(0.0));
+	}
+
+  /**@brief Multiply this Transform by another(this = this * another) 
+   * @param t The other transform */
+	Transform& operator*=(const Transform& t) 
+	{
+		m_origin += m_basis * t.m_origin;
+		m_basis *= t.m_basis;
+		return *this;
+	}
+
+  /**@brief Return the inverse of this transform */
+	Transform inverse() const
+	{ 
+		Matrix3x3 inv = m_basis.transpose();
+		return Transform(inv, inv * -m_origin);
+	}
+
+  /**@brief Return the inverse of this transform times the other transform
+   * @param t The other transform 
+   * return this.inverse() * the other */
+	Transform inverseTimes(const Transform& t) const;  
+
+  /**@brief Return the product of this transform and the other */
+	Transform operator*(const Transform& t) const;
+
+  /**@brief Return an identity transform */
+	static const Transform&	getIdentity()
+	{
+		static const Transform identityTransform(Matrix3x3::getIdentity());
+		return identityTransform;
+	}
+
+	void	serialize(struct	TransformData& dataOut) const;
+
+	void	serializeFloat(struct	TransformFloatData& dataOut) const;
+
+	void	deSerialize(const struct	TransformData& dataIn);
+
+	void	deSerializeDouble(const struct	TransformDoubleData& dataIn);
+
+	void	deSerializeFloat(const struct	TransformFloatData& dataIn);
+
+};
+
+
+TF3SIMD_FORCE_INLINE Vector3
+Transform::invXform(const Vector3& inVec) const
+{
+	Vector3 v = inVec - m_origin;
+	return (m_basis.transpose() * v);
+}
+
+TF3SIMD_FORCE_INLINE Transform 
+Transform::inverseTimes(const Transform& t) const  
+{
+	Vector3 v = t.getOrigin() - m_origin;
+		return Transform(m_basis.transposeTimes(t.m_basis),
+			v * m_basis);
+}
+
+TF3SIMD_FORCE_INLINE Transform 
+Transform::operator*(const Transform& t) const
+{
+	return Transform(m_basis * t.m_basis, 
+		(*this)(t.m_origin));
+}
+
+/**@brief Test if two transforms have all elements equal */
+TF3SIMD_FORCE_INLINE bool operator==(const Transform& t1, const Transform& t2)
+{
+   return ( t1.getBasis()  == t2.getBasis() &&
+            t1.getOrigin() == t2.getOrigin() );
+}
+
+
+///for serialization
+struct	TransformFloatData
+{
+	Matrix3x3FloatData	m_basis;
+	Vector3FloatData	m_origin;
+};
+
+struct	TransformDoubleData
+{
+	Matrix3x3DoubleData	m_basis;
+	Vector3DoubleData	m_origin;
+};
+
+
+
+TF3SIMD_FORCE_INLINE	void	Transform::serialize(TransformData& dataOut) const
+{
+	m_basis.serialize(dataOut.m_basis);
+	m_origin.serialize(dataOut.m_origin);
+}
+
+TF3SIMD_FORCE_INLINE	void	Transform::serializeFloat(TransformFloatData& dataOut) const
+{
+	m_basis.serializeFloat(dataOut.m_basis);
+	m_origin.serializeFloat(dataOut.m_origin);
+}
+
+
+TF3SIMD_FORCE_INLINE	void	Transform::deSerialize(const TransformData& dataIn)
+{
+	m_basis.deSerialize(dataIn.m_basis);
+	m_origin.deSerialize(dataIn.m_origin);
+}
+
+TF3SIMD_FORCE_INLINE	void	Transform::deSerializeFloat(const TransformFloatData& dataIn)
+{
+	m_basis.deSerializeFloat(dataIn.m_basis);
+	m_origin.deSerializeFloat(dataIn.m_origin);
+}
+
+TF3SIMD_FORCE_INLINE	void	Transform::deSerializeDouble(const TransformDoubleData& dataIn)
+{
+	m_basis.deSerializeDouble(dataIn.m_basis);
+	m_origin.deSerializeDouble(dataIn.m_origin);
+}
+
+}
+
+#endif
+
+
+
+
+
+

tf3-main/include/tf3/LinearMath/Vector3.h

@@ -0,0 +1,731 @@
+/*
+Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#ifndef TF3_VECTOR3_H
+#define TF3_VECTOR3_H
+
+
+#include "Scalar.h"
+#include "MinMax.h"
+
+namespace tf3
+{
+
+#define Vector3Data Vector3DoubleData
+#define Vector3DataName "Vector3DoubleData"
+
+
+
+
+/**@brief tf3::Vector3 can be used to represent 3D points and vectors.
+ * It has an un-used w component to suit 16-byte alignment when tf3::Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
+ * Ideally, this class should be replaced by a platform optimized TF3SIMD version that keeps the data in registers
+ */
+ATTRIBUTE_ALIGNED16(class) Vector3
+{
+public:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+		tf3Scalar	m_floats[4];
+public:
+	TF3SIMD_FORCE_INLINE const vec_float4&	get128() const
+	{
+		return *((const vec_float4*)&m_floats[0]);
+	}
+public:
+#else //__CELLOS_LV2__ __SPU__
+#ifdef TF3_USE_SSE // _WIN32
+	union {
+		__m128 mVec128;
+		tf3Scalar	m_floats[4];
+	};
+	TF3SIMD_FORCE_INLINE	__m128	get128() const
+	{
+		return mVec128;
+	}
+	TF3SIMD_FORCE_INLINE	void	set128(__m128 v128)
+	{
+		mVec128 = v128;
+	}
+#else
+	tf3Scalar	m_floats[4];
+#endif
+#endif //__CELLOS_LV2__ __SPU__
+
+	public:
+
+  /**@brief No initialization constructor */
+	TF3SIMD_FORCE_INLINE Vector3() {}
+
+ 
+	
+  /**@brief Constructor from scalars 
+   * @param x X value
+   * @param y Y value 
+   * @param z Z value 
+   */
+	TF3SIMD_FORCE_INLINE Vector3(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z)
+	{
+		m_floats[0] = x;
+		m_floats[1] = y;
+		m_floats[2] = z;
+		m_floats[3] = tf3Scalar(0.);
+	}
+
+/**@brief Add a vector to this one 
+ * @param The vector to add to this one */
+	TF3SIMD_FORCE_INLINE Vector3& operator+=(const Vector3& v)
+	{
+
+		m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
+		return *this;
+	}
+
+
+  /**@brief Sutf3ract a vector from this one
+   * @param The vector to sutf3ract */
+	TF3SIMD_FORCE_INLINE Vector3& operator-=(const Vector3& v) 
+	{
+		m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
+		return *this;
+	}
+  /**@brief Scale the vector
+   * @param s Scale factor */
+	TF3SIMD_FORCE_INLINE Vector3& operator*=(const tf3Scalar& s)
+	{
+		m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
+		return *this;
+	}
+
+  /**@brief Inversely scale the vector 
+   * @param s Scale factor to divide by */
+	TF3SIMD_FORCE_INLINE Vector3& operator/=(const tf3Scalar& s) 
+	{
+		tf3FullAssert(s != tf3Scalar(0.0));
+		return *this *= tf3Scalar(1.0) / s;
+	}
+
+  /**@brief Return the dot product
+   * @param v The other vector in the dot product */
+	TF3SIMD_FORCE_INLINE tf3Scalar dot(const Vector3& v) const
+	{
+		return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
+	}
+
+  /**@brief Return the length of the vector squared */
+	TF3SIMD_FORCE_INLINE tf3Scalar length2() const
+	{
+		return dot(*this);
+	}
+
+  /**@brief Return the length of the vector */
+	TF3SIMD_FORCE_INLINE tf3Scalar length() const
+	{
+		return tf3Sqrt(length2());
+	}
+
+  /**@brief Return the distance squared between the ends of this and another vector
+   * This is symantically treating the vector like a point */
+	TF3SIMD_FORCE_INLINE tf3Scalar distance2(const Vector3& v) const;
+
+  /**@brief Return the distance between the ends of this and another vector
+   * This is symantically treating the vector like a point */
+	TF3SIMD_FORCE_INLINE tf3Scalar distance(const Vector3& v) const;
+
+  /**@brief Normalize this vector 
+   * x^2 + y^2 + z^2 = 1 */
+	TF3SIMD_FORCE_INLINE Vector3& normalize() 
+	{
+		return *this /= length();
+	}
+
+  /**@brief Return a normalized version of this vector */
+	TF3SIMD_FORCE_INLINE Vector3 normalized() const;
+
+  /**@brief Rotate this vector 
+   * @param wAxis The axis to rotate about 
+   * @param angle The angle to rotate by */
+	TF3SIMD_FORCE_INLINE Vector3 rotate( const Vector3& wAxis, const tf3Scalar angle ) const;
+
+  /**@brief Return the angle between this and another vector
+   * @param v The other vector */
+	TF3SIMD_FORCE_INLINE tf3Scalar angle(const Vector3& v) const 
+	{
+		tf3Scalar s = tf3Sqrt(length2() * v.length2());
+		tf3FullAssert(s != tf3Scalar(0.0));
+		return tf3Acos(dot(v) / s);
+	}
+  /**@brief Return a vector will the absolute values of each element */
+	TF3SIMD_FORCE_INLINE Vector3 absolute() const 
+	{
+		return Vector3(
+			tf3Fabs(m_floats[0]), 
+			tf3Fabs(m_floats[1]), 
+			tf3Fabs(m_floats[2]));
+	}
+  /**@brief Return the cross product between this and another vector 
+   * @param v The other vector */
+	TF3SIMD_FORCE_INLINE Vector3 cross(const Vector3& v) const
+	{
+		return Vector3(
+			m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
+			m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+			m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
+	}
+
+	TF3SIMD_FORCE_INLINE tf3Scalar triple(const Vector3& v1, const Vector3& v2) const
+	{
+		return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + 
+			m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + 
+			m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
+	}
+
+  /**@brief Return the axis with the smallest value 
+   * Note return values are 0,1,2 for x, y, or z */
+	TF3SIMD_FORCE_INLINE int minAxis() const
+	{
+		return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
+	}
+
+  /**@brief Return the axis with the largest value 
+   * Note return values are 0,1,2 for x, y, or z */
+	TF3SIMD_FORCE_INLINE int maxAxis() const 
+	{
+		return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
+	}
+
+	TF3SIMD_FORCE_INLINE int furthestAxis() const
+	{
+		return absolute().minAxis();
+	}
+
+	TF3SIMD_FORCE_INLINE int closestAxis() const 
+	{
+		return absolute().maxAxis();
+	}
+
+	TF3SIMD_FORCE_INLINE void setInterpolate3(const Vector3& v0, const Vector3& v1, tf3Scalar rt)
+	{
+		tf3Scalar s = tf3Scalar(1.0) - rt;
+		m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+		m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+		m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
+		//don't do the unused w component
+		//		m_co[3] = s * v0[3] + rt * v1[3];
+	}
+
+  /**@brief Return the linear interpolation between this and another vector 
+   * @param v The other vector 
+   * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
+	TF3SIMD_FORCE_INLINE Vector3 lerp(const Vector3& v, const tf3Scalar& t) const 
+	{
+		return Vector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+			m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+			m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
+	}
+
+  /**@brief Elementwise multiply this vector by the other 
+   * @param v The other vector */
+	TF3SIMD_FORCE_INLINE Vector3& operator*=(const Vector3& v)
+	{
+		m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
+		return *this;
+	}
+
+	 /**@brief Return the x value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& getX() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& getY() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& getZ() const { return m_floats[2]; }
+  /**@brief Set the x value */
+		TF3SIMD_FORCE_INLINE void	setX(tf3Scalar x) { m_floats[0] = x;};
+  /**@brief Set the y value */
+		TF3SIMD_FORCE_INLINE void	setY(tf3Scalar y) { m_floats[1] = y;};
+  /**@brief Set the z value */
+		TF3SIMD_FORCE_INLINE void	setZ(tf3Scalar z) {m_floats[2] = z;};
+  /**@brief Set the w value */
+		TF3SIMD_FORCE_INLINE void	setW(tf3Scalar w) { m_floats[3] = w;};
+  /**@brief Return the x value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& x() const { return m_floats[0]; }
+  /**@brief Return the y value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& y() const { return m_floats[1]; }
+  /**@brief Return the z value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& z() const { return m_floats[2]; }
+  /**@brief Return the w value */
+		TF3SIMD_FORCE_INLINE const tf3Scalar& w() const { return m_floats[3]; }
+
+	//TF3SIMD_FORCE_INLINE tf3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}      
+	//TF3SIMD_FORCE_INLINE const tf3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator tf3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	TF3SIMD_FORCE_INLINE	operator       tf3Scalar *()       { return &m_floats[0]; }
+	TF3SIMD_FORCE_INLINE	operator const tf3Scalar *() const { return &m_floats[0]; }
+
+	TF3SIMD_FORCE_INLINE	bool	operator==(const Vector3& other) const
+	{
+		return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
+	}
+
+	TF3SIMD_FORCE_INLINE	bool	operator!=(const Vector3& other) const
+	{
+		return !(*this == other);
+	}
+
+	 /**@brief Set each element to the max of the current values and the values of another Vector3
+   * @param other The other Vector3 to compare with 
+   */
+		TF3SIMD_FORCE_INLINE void	setMax(const Vector3& other)
+		{
+			tf3SetMax(m_floats[0], other.m_floats[0]);
+			tf3SetMax(m_floats[1], other.m_floats[1]);
+			tf3SetMax(m_floats[2], other.m_floats[2]);
+			tf3SetMax(m_floats[3], other.w());
+		}
+  /**@brief Set each element to the min of the current values and the values of another Vector3
+   * @param other The other Vector3 to compare with 
+   */
+		TF3SIMD_FORCE_INLINE void	setMin(const Vector3& other)
+		{
+			tf3SetMin(m_floats[0], other.m_floats[0]);
+			tf3SetMin(m_floats[1], other.m_floats[1]);
+			tf3SetMin(m_floats[2], other.m_floats[2]);
+			tf3SetMin(m_floats[3], other.w());
+		}
+
+		TF3SIMD_FORCE_INLINE void 	setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z)
+		{
+			m_floats[0]=x;
+			m_floats[1]=y;
+			m_floats[2]=z;
+			m_floats[3] = tf3Scalar(0.);
+		}
+
+		void	getSkewSymmetricMatrix(Vector3* v0,Vector3* v1,Vector3* v2) const
+		{
+			v0->setValue(0.		,-z()		,y());
+			v1->setValue(z()	,0.			,-x());
+			v2->setValue(-y()	,x()	,0.);
+		}
+
+		void	setZero()
+		{
+			setValue(tf3Scalar(0.),tf3Scalar(0.),tf3Scalar(0.));
+		}
+
+		TF3SIMD_FORCE_INLINE bool isZero() const 
+		{
+			return m_floats[0] == tf3Scalar(0) && m_floats[1] == tf3Scalar(0) && m_floats[2] == tf3Scalar(0);
+		}
+
+		TF3SIMD_FORCE_INLINE bool fuzzyZero() const 
+		{
+			return length2() < TF3SIMD_EPSILON;
+		}
+
+		TF3SIMD_FORCE_INLINE	void	serialize(struct	Vector3Data& dataOut) const;
+
+		TF3SIMD_FORCE_INLINE	void	deSerialize(const struct	Vector3Data& dataIn);
+
+		TF3SIMD_FORCE_INLINE	void	serializeFloat(struct	Vector3FloatData& dataOut) const;
+
+		TF3SIMD_FORCE_INLINE	void	deSerializeFloat(const struct	Vector3FloatData& dataIn);
+
+		TF3SIMD_FORCE_INLINE	void	serializeDouble(struct	Vector3DoubleData& dataOut) const;
+
+		TF3SIMD_FORCE_INLINE	void	deSerializeDouble(const struct	Vector3DoubleData& dataIn);
+
+};
+
+/**@brief Return the sum of two vectors (Point symantics)*/
+TF3SIMD_FORCE_INLINE Vector3 
+operator+(const Vector3& v1, const Vector3& v2) 
+{
+	return Vector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
+}
+
+/**@brief Return the elementwise product of two vectors */
+TF3SIMD_FORCE_INLINE Vector3 
+operator*(const Vector3& v1, const Vector3& v2) 
+{
+	return Vector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
+}
+
+/**@brief Return the difference between two vectors */
+TF3SIMD_FORCE_INLINE Vector3 
+operator-(const Vector3& v1, const Vector3& v2)
+{
+	return Vector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
+}
+/**@brief Return the negative of the vector */
+TF3SIMD_FORCE_INLINE Vector3 
+operator-(const Vector3& v)
+{
+	return Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
+}
+
+/**@brief Return the vector scaled by s */
+TF3SIMD_FORCE_INLINE Vector3 
+operator*(const Vector3& v, const tf3Scalar& s)
+{
+	return Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
+}
+
+/**@brief Return the vector scaled by s */
+TF3SIMD_FORCE_INLINE Vector3 
+operator*(const tf3Scalar& s, const Vector3& v)
+{ 
+	return v * s; 
+}
+
+/**@brief Return the vector inversely scaled by s */
+TF3SIMD_FORCE_INLINE Vector3
+operator/(const Vector3& v, const tf3Scalar& s)
+{
+	tf3FullAssert(s != tf3Scalar(0.0));
+	return v * (tf3Scalar(1.0) / s);
+}
+
+/**@brief Return the vector inversely scaled by s */
+TF3SIMD_FORCE_INLINE Vector3
+operator/(const Vector3& v1, const Vector3& v2)
+{
+	return Vector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
+}
+
+/**@brief Return the dot product between two vectors */
+TF3SIMD_FORCE_INLINE tf3Scalar 
+tf3Dot(const Vector3& v1, const Vector3& v2) 
+{ 
+	return v1.dot(v2); 
+}
+
+
+/**@brief Return the distance squared between two vectors */
+TF3SIMD_FORCE_INLINE tf3Scalar
+tf3Distance2(const Vector3& v1, const Vector3& v2) 
+{ 
+	return v1.distance2(v2); 
+}
+
+
+/**@brief Return the distance between two vectors */
+TF3SIMD_FORCE_INLINE tf3Scalar
+tf3Distance(const Vector3& v1, const Vector3& v2) 
+{ 
+	return v1.distance(v2); 
+}
+
+/**@brief Return the angle between two vectors */
+TF3SIMD_FORCE_INLINE tf3Scalar
+tf3Angle(const Vector3& v1, const Vector3& v2) 
+{ 
+	return v1.angle(v2); 
+}
+
+/**@brief Return the cross product of two vectors */
+TF3SIMD_FORCE_INLINE Vector3 
+tf3Cross(const Vector3& v1, const Vector3& v2) 
+{ 
+	return v1.cross(v2); 
+}
+
+TF3SIMD_FORCE_INLINE tf3Scalar
+tf3Triple(const Vector3& v1, const Vector3& v2, const Vector3& v3)
+{
+	return v1.triple(v2, v3);
+}
+
+/**@brief Return the linear interpolation between two vectors
+ * @param v1 One vector 
+ * @param v2 The other vector 
+ * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
+TF3SIMD_FORCE_INLINE Vector3 
+lerp(const Vector3& v1, const Vector3& v2, const tf3Scalar& t)
+{
+	return v1.lerp(v2, t);
+}
+
+
+
+TF3SIMD_FORCE_INLINE tf3Scalar Vector3::distance2(const Vector3& v) const
+{
+	return (v - *this).length2();
+}
+
+TF3SIMD_FORCE_INLINE tf3Scalar Vector3::distance(const Vector3& v) const
+{
+	return (v - *this).length();
+}
+
+TF3SIMD_FORCE_INLINE Vector3 Vector3::normalized() const
+{
+	return *this / length();
+} 
+
+TF3SIMD_FORCE_INLINE Vector3 Vector3::rotate( const Vector3& wAxis, const tf3Scalar angle ) const
+{
+	// wAxis must be a unit lenght vector
+
+	Vector3 o = wAxis * wAxis.dot( *this );
+	Vector3 x = *this - o;
+	Vector3 y;
+
+	y = wAxis.cross( *this );
+
+	return ( o + x * tf3Cos( angle ) + y * tf3Sin( angle ) );
+}
+
+class tf3Vector4 : public Vector3
+{
+public:
+
+	TF3SIMD_FORCE_INLINE tf3Vector4() {}
+
+
+	TF3SIMD_FORCE_INLINE tf3Vector4(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w) 
+		: Vector3(x,y,z)
+	{
+		m_floats[3] = w;
+	}
+
+
+	TF3SIMD_FORCE_INLINE tf3Vector4 absolute4() const 
+	{
+		return tf3Vector4(
+			tf3Fabs(m_floats[0]), 
+			tf3Fabs(m_floats[1]), 
+			tf3Fabs(m_floats[2]),
+			tf3Fabs(m_floats[3]));
+	}
+
+
+
+	tf3Scalar	getW() const { return m_floats[3];}
+
+
+		TF3SIMD_FORCE_INLINE int maxAxis4() const
+	{
+		int maxIndex = -1;
+		tf3Scalar maxVal = tf3Scalar(-TF3_LARGE_FLOAT);
+		if (m_floats[0] > maxVal)
+		{
+			maxIndex = 0;
+			maxVal = m_floats[0];
+		}
+		if (m_floats[1] > maxVal)
+		{
+			maxIndex = 1;
+			maxVal = m_floats[1];
+		}
+		if (m_floats[2] > maxVal)
+		{
+			maxIndex = 2;
+			maxVal =m_floats[2];
+		}
+		if (m_floats[3] > maxVal)
+		{
+			maxIndex = 3;
+		}
+		
+		
+		
+
+		return maxIndex;
+
+	}
+
+
+	TF3SIMD_FORCE_INLINE int minAxis4() const
+	{
+		int minIndex = -1;
+		tf3Scalar minVal = tf3Scalar(TF3_LARGE_FLOAT);
+		if (m_floats[0] < minVal)
+		{
+			minIndex = 0;
+			minVal = m_floats[0];
+		}
+		if (m_floats[1] < minVal)
+		{
+			minIndex = 1;
+			minVal = m_floats[1];
+		}
+		if (m_floats[2] < minVal)
+		{
+			minIndex = 2;
+			minVal =m_floats[2];
+		}
+		if (m_floats[3] < minVal)
+		{
+			minIndex = 3;
+		}
+		
+		return minIndex;
+
+	}
+
+
+	TF3SIMD_FORCE_INLINE int closestAxis4() const 
+	{
+		return absolute4().maxAxis4();
+	}
+
+	
+ 
+
+  /**@brief Set x,y,z and zero w 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+		
+
+/*		void getValue(tf3Scalar *m) const 
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] =m_floats[2];
+		}
+*/
+/**@brief Set the values 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+		TF3SIMD_FORCE_INLINE void	setValue(const tf3Scalar& x, const tf3Scalar& y, const tf3Scalar& z,const tf3Scalar& w)
+		{
+			m_floats[0]=x;
+			m_floats[1]=y;
+			m_floats[2]=z;
+			m_floats[3]=w;
+		}
+
+
+};
+
+
+///tf3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+TF3SIMD_FORCE_INLINE void	tf3SwapScalarEndian(const tf3Scalar& sourceVal, tf3Scalar& destVal)
+{
+	unsigned char* dest = (unsigned char*) &destVal;
+	const unsigned char* src  = (const unsigned char*) &sourceVal;
+	dest[0] = src[7];
+    dest[1] = src[6];
+    dest[2] = src[5];
+    dest[3] = src[4];
+    dest[4] = src[3];
+    dest[5] = src[2];
+    dest[6] = src[1];
+    dest[7] = src[0];
+}
+///tf3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+TF3SIMD_FORCE_INLINE void	tf3SwapVector3Endian(const Vector3& sourceVec, Vector3& destVec)
+{
+	for (int i=0;i<4;i++)
+	{
+		tf3SwapScalarEndian(sourceVec[i],destVec[i]);
+	}
+
+}
+
+///tf3UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
+TF3SIMD_FORCE_INLINE void	tf3UnSwapVector3Endian(Vector3& vector)
+{
+
+	Vector3	swappedVec;
+	for (int i=0;i<4;i++)
+	{
+		tf3SwapScalarEndian(vector[i],swappedVec[i]);
+	}
+	vector = swappedVec;
+}
+
+TF3SIMD_FORCE_INLINE void tf3PlaneSpace1 (const Vector3& n, Vector3& p, Vector3& q)
+{
+  if (tf3Fabs(n.z()) > TF3SIMDSQRT12) {
+    // choose p in y-z plane
+    tf3Scalar a = n[1]*n[1] + n[2]*n[2];
+    tf3Scalar k = tf3RecipSqrt (a);
+    p.setValue(0,-n[2]*k,n[1]*k);
+    // set q = n x p
+    q.setValue(a*k,-n[0]*p[2],n[0]*p[1]);
+  }
+  else {
+    // choose p in x-y plane
+    tf3Scalar a = n.x()*n.x() + n.y()*n.y();
+    tf3Scalar k = tf3RecipSqrt (a);
+    p.setValue(-n.y()*k,n.x()*k,0);
+    // set q = n x p
+    q.setValue(-n.z()*p.y(),n.z()*p.x(),a*k);
+  }
+}
+
+
+struct	Vector3FloatData
+{
+	float	m_floats[4];
+};
+
+struct	Vector3DoubleData
+{
+	double	m_floats[4];
+
+};
+
+TF3SIMD_FORCE_INLINE	void	Vector3::serializeFloat(struct	Vector3FloatData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = float(m_floats[i]);
+}
+
+TF3SIMD_FORCE_INLINE void	Vector3::deSerializeFloat(const struct	Vector3FloatData& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = tf3Scalar(dataIn.m_floats[i]);
+}
+
+
+TF3SIMD_FORCE_INLINE	void	Vector3::serializeDouble(struct	Vector3DoubleData& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = double(m_floats[i]);
+}
+
+TF3SIMD_FORCE_INLINE void	Vector3::deSerializeDouble(const struct	Vector3DoubleData& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = tf3Scalar(dataIn.m_floats[i]);
+}
+
+
+TF3SIMD_FORCE_INLINE	void	Vector3::serialize(struct	Vector3Data& dataOut) const
+{
+	///could also do a memcpy, check if it is worth it
+	for (int i=0;i<4;i++)
+		dataOut.m_floats[i] = m_floats[i];
+}
+
+TF3SIMD_FORCE_INLINE void	Vector3::deSerialize(const struct	Vector3Data& dataIn)
+{
+	for (int i=0;i<4;i++)
+		m_floats[i] = dataIn.m_floats[i];
+}
+
+}
+
+#endif //TF3_VECTOR3_H

tf3-main/include/tf3/TF3Error.h

@@ -0,0 +1,11 @@
+// TF3 error codes in tf3 namespace (no tf3_msgs dependency)
+#ifndef TF3_ERROR_TF3_H
+#define TF3_ERROR_TF3_H
+
+namespace tf3 {
+struct TF3Error {
+  enum { NO_ERROR = 0, LOOKUP_ERROR = 1, CONNECTIVITY_ERROR = 2, EXTRAPOLATION_ERROR = 3 };
+};
+}
+
+#endif // TF3_ERROR_TF3_H

tf3-main/include/tf3/buffer_core.h

@@ -0,0 +1,432 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#ifndef TF3_BUFFER_CORE_H
+#define TF3_BUFFER_CORE_H
+
+#include "transform_storage.h"
+
+#include <boost/signals2.hpp>
+
+#include <string>
+
+#include "tf3/compat.h"
+#include "tf3/time.h"
+
+
+//////////////////////////backwards startup for porting
+//#include "tf/tf.h"
+
+#include <boost/unordered_map.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/function.hpp>
+#include <boost/shared_ptr.hpp>
+
+namespace tf3
+{
+
+typedef std::pair<tf3::Time, CompactFrameID> P_TimeAndFrameID;
+typedef uint32_t TransformableCallbackHandle;
+typedef uint64_t TransformableRequestHandle;
+
+class TimeCacheInterface;
+typedef boost::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
+
+enum TransformableResult
+{
+  TransformAvailable,
+  TransformFailure,
+};
+
+/** \brief A Class which provides coordinate transforms between any two frames in a system.
+ *
+ * This class provides a simple interface to allow recording and lookup of
+ * relationships between arbitrary frames of the system.
+ *
+ * libTF assumes that there is a tree of coordinate frame transforms which define the relationship between all coordinate frames.
+ * For example your typical robot would have a transform from global to real world.  And then from base to hand, and from base to head.
+ * But Base to Hand really is composed of base to shoulder to elbow to wrist to hand.
+ * libTF is designed to take care of all the intermediate steps for you.
+ *
+ * Internal Representation
+ * libTF will store frames with the parameters necessary for generating the transform into that frame from it's parent and a reference to the parent frame.
+ * Frames are designated using an std::string
+ * 0 is a frame without a parent (the top of a tree)
+ * The positions of frames over time must be pushed in.
+ *
+ * All function calls which pass frame ids can potentially throw the exception tf::LookupException
+ */
+class BufferCore
+{
+public:
+  /************* Constants ***********************/
+  static const int DEFAULT_CACHE_TIME = 10;  //!< The default amount of time to cache data in seconds
+  static const uint32_t MAX_GRAPH_DEPTH = 1000UL;  //!< Maximum graph search depth (deeper graphs will be assumed to have loops)
+
+  /** Constructor
+   * \param interpolating Whether to interpolate, if this is false the closest value will be returned
+   * \param cache_time How long to keep a history of transforms in nanoseconds
+   *
+   */
+  BufferCore(tf3::Duration cache_time_ = tf3::Duration(DEFAULT_CACHE_TIME));
+  virtual ~BufferCore(void);
+
+  /** \brief Clear all data */
+  void clear();
+
+  /** \brief Add transform information to the tf data structure
+   * \param transform The transform to store
+   * \param authority The source of the information for this transform
+   * \param is_static Record this transform as a static transform.  It will be good across all time.  (This cannot be changed after the first call.)
+   * \return True unless an error occured
+   */
+  bool setTransform(const tf3::TransformStampedMsg& transform, const std::string & authority, bool is_static = false);
+
+  /*********** Accessors *************/
+
+  /** \brief Get the transform between two frames by frame ID.
+   * \param target_frame The frame to which data should be transformed
+   * \param source_frame The frame where the data originated
+   * \param time The time at which the value of the transform is desired. (0 will get the latest)
+   * \return The transform between the frames
+   *
+   * Possible exceptions tf3::LookupException, tf3::ConnectivityException,
+   * tf3::ExtrapolationException, tf3::InvalidArgumentException
+   */
+  tf3::TransformStampedMsg 
+  lookupTransform(const std::string& target_frame, const std::string& source_frame,
+  const tf3::Time& time) const;
+
+  /** \brief Get the transform between two frames by frame ID assuming fixed frame.
+   * \param target_frame The frame to which data should be transformed
+   * \param target_time The time to which the data should be transformed. (0 will get the latest)
+   * \param source_frame The frame where the data originated
+   * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
+   * \param fixed_frame The frame in which to assume the transform is constant in time. 
+   * \return The transform between the frames
+   *
+   * Possible exceptions tf3::LookupException, tf3::ConnectivityException,
+   * tf3::ExtrapolationException, tf3::InvalidArgumentException
+   */
+
+  tf3::TransformStampedMsg
+  lookupTransform(const std::string& target_frame, const tf3::Time& target_time,
+  const std::string& source_frame, const tf3::Time& source_time,
+  const std::string& fixed_frame) const;
+  
+  /* \brief Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point
+   * \param tracking_frame The frame to track
+   * \param observation_frame The frame from which to measure the twist
+   * \param reference_frame The reference frame in which to express the twist
+   * \param reference_point The reference point with which to express the twist
+   * \param reference_point_frame The frame_id in which the reference point is expressed
+   * \param time The time at which to get the velocity
+   * \param duration The period over which to average
+   * \return twist The twist output
+   * 
+   * This will compute the average velocity on the interval 
+   * (time - duration/2, time+duration/2). If that is too close to the most
+   * recent reading, in which case it will shift the interval up to
+   * duration/2 to prevent extrapolation.
+   *
+   * Possible exceptions tf3::LookupException, tf3::ConnectivityException,
+   * tf3::ExtrapolationException, tf3::InvalidArgumentException
+   * 
+   * New in geometry 1.1
+   */
+  /*
+  geometry_msgs::Twist
+    lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
+		const tf::Point & reference_point, const std::string& reference_point_frame, 
+		const ros::Time& time, const ros::Duration& averaging_interval) const;
+  */
+  /* \brief lookup the twist of the tracking frame with respect to the observational frame
+   * 
+   * This is a simplified version of
+   * lookupTwist with it assumed that the reference point is the
+   * origin of the tracking frame, and the reference frame is the
+   * observation frame.  
+   *
+   * Possible exceptions tf3::LookupException, tf3::ConnectivityException,
+   * tf3::ExtrapolationException, tf3::InvalidArgumentException
+   * 
+   * New in geometry 1.1
+   */
+  /*
+  geometry_msgs::Twist
+    lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, 
+		const ros::Time& time, const ros::Duration& averaging_interval) const;
+  */
+  /** \brief Test if a transform is possible
+   * \param target_frame The frame into which to transform
+   * \param source_frame The frame from which to transform
+   * \param time The time at which to transform
+   * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL
+   * \return True if the transform is possible, false otherwise 
+   */
+  bool canTransform(const std::string& target_frame, const std::string& source_frame,
+                  const tf3::Time& time, std::string* error_msg = NULL) const;
+  
+  /** \brief Test if a transform is possible
+   * \param target_frame The frame into which to transform
+   * \param target_time The time into which to transform
+   * \param source_frame The frame from which to transform
+   * \param source_time The time from which to transform
+   * \param fixed_frame The frame in which to treat the transform as constant in time
+   * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL
+   * \return True if the transform is possible, false otherwise 
+   */
+  bool canTransform(const std::string& target_frame, const tf3::Time& target_time,
+                    const std::string& source_frame, const tf3::Time& source_time,
+                    const std::string& fixed_frame, std::string* error_msg = NULL) const;
+
+  /** \brief A way to see what frames have been cached in yaml format
+   * Useful for debugging tools
+   */
+  std::string allFramesAsYAML(double current_time) const;
+
+  /** Backwards compatibility for #84
+  */
+  std::string allFramesAsYAML() const;
+
+  /** \brief A way to see what frames have been cached
+   * Useful for debugging
+   */
+  std::string allFramesAsString() const;
+  
+  typedef boost::function<void(TransformableRequestHandle request_handle, const std::string& target_frame, const std::string& source_frame,
+                               tf3::Time time, TransformableResult result)> TransformableCallback;
+
+  /// \brief Internal use only
+  TransformableCallbackHandle addTransformableCallback(const TransformableCallback& cb);
+  /// \brief Internal use only
+  void removeTransformableCallback(TransformableCallbackHandle handle);
+  /// \brief Internal use only
+  TransformableRequestHandle addTransformableRequest(TransformableCallbackHandle handle, const std::string& target_frame, const std::string& source_frame, tf3::Time time);
+  /// \brief Internal use only
+  void cancelTransformableRequest(TransformableRequestHandle handle);
+
+
+
+
+  // Tell the buffer that there are multiple threads serviciing it. 
+  // This is useful for derived classes to know if they can block or not. 
+  void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;};
+  // Get the state of using_dedicated_thread_
+  bool isUsingDedicatedThread() const { return using_dedicated_thread_;};
+  
+
+
+
+  /* Backwards compatability section for tf::Transformer you should not use these
+   */
+
+  /**
+   * \brief Add a callback that happens when a new transform has arrived
+   *
+   * \param callback The callback, of the form void func();
+   * \return A boost::signals2::connection object that can be used to remove this
+   * listener
+   */
+  boost::signals2::connection _addTransformsChangedListener(boost::function<void(void)> callback);
+  void _removeTransformsChangedListener(boost::signals2::connection c);
+
+
+  /**@brief Check if a frame exists in the tree
+   * @param frame_id_str The frame id in question  */
+  bool _frameExists(const std::string& frame_id_str) const;
+
+  /**@brief Fill the parent of a frame.
+   * @param frame_id The frame id of the frame in question
+   * @param parent The reference to the string to fill the parent
+   * Returns true unless "NO_PARENT" */
+  bool _getParent(const std::string& frame_id, tf3::Time time, std::string& parent) const;
+
+  /** \brief A way to get a std::vector of available frame ids */
+  void _getFrameStrings(std::vector<std::string>& ids) const;
+
+
+  CompactFrameID _lookupFrameNumber(const std::string& frameid_str) const { 
+    return lookupFrameNumber(frameid_str); 
+  }
+  CompactFrameID _lookupOrInsertFrameNumber(const std::string& frameid_str) {
+    return lookupOrInsertFrameNumber(frameid_str); 
+  }
+
+  int _getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, tf3::Time& time, std::string* error_string) const {
+    boost::mutex::scoped_lock lock(frame_mutex_);
+    return getLatestCommonTime(target_frame, source_frame, time, error_string);
+  }
+
+  CompactFrameID _validateFrameId(const char* function_name_arg, const std::string& frame_id) const {
+    return validateFrameId(function_name_arg, frame_id);
+  }
+
+  /**@brief Get the duration over which this transformer will cache */
+  tf3::Duration getCacheLength() { return cache_time_;}
+
+  /** \brief Backwards compatabilityA way to see what frames have been cached
+   * Useful for debugging
+   */
+  std::string _allFramesAsDot(double current_time) const;
+  std::string _allFramesAsDot() const;
+
+  /** \brief Backwards compatabilityA way to see what frames are in a chain
+   * Useful for debugging
+   */
+  void _chainAsVector(const std::string & target_frame, tf3::Time target_time, const std::string & source_frame, tf3::Time source_time, const std::string & fixed_frame, std::vector<std::string>& output) const;
+
+private:
+
+  /** \brief A way to see what frames have been cached
+   * Useful for debugging. Use this call internally. 
+   */
+  std::string allFramesAsStringNoLock() const;  
+
+
+  /******************** Internal Storage ****************/
+  
+  /** \brief The pointers to potential frames that the tree can be made of.
+   * The frames will be dynamically allocated at run time when set the first time. */
+  typedef std::vector<TimeCacheInterfacePtr> V_TimeCacheInterface;
+  V_TimeCacheInterface frames_;
+  
+  /** \brief A mutex to protect testing and allocating new frames on the above vector. */
+  mutable boost::mutex frame_mutex_;
+
+  /** \brief A map from string frame ids to CompactFrameID */
+  typedef boost::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
+  M_StringToCompactFrameID frameIDs_;
+  /** \brief A map from CompactFrameID frame_id_numbers to string for debugging and output */
+  std::vector<std::string> frameIDs_reverse;
+  /** \brief A map to lookup the most recent authority for a given frame */
+  std::map<CompactFrameID, std::string> frame_authority_;
+
+
+  /// How long to cache transform history
+  tf3::Duration cache_time_;
+
+  typedef boost::unordered_map<TransformableCallbackHandle, TransformableCallback> M_TransformableCallback;
+  M_TransformableCallback transformable_callbacks_;
+  uint32_t transformable_callbacks_counter_;
+  boost::mutex transformable_callbacks_mutex_;
+
+  struct TransformableRequest
+  {
+  tf3::Time time;
+    TransformableRequestHandle request_handle;
+    TransformableCallbackHandle cb_handle;
+    CompactFrameID target_id;
+    CompactFrameID source_id;
+    std::string target_string;
+    std::string source_string;
+  };
+  typedef std::vector<TransformableRequest> V_TransformableRequest;
+  V_TransformableRequest transformable_requests_;
+  boost::mutex transformable_requests_mutex_;
+  uint64_t transformable_requests_counter_;
+
+  struct RemoveRequestByCallback;
+  struct RemoveRequestByID;
+
+  // Backwards compatability for tf message_filter
+  typedef boost::signals2::signal<void(void)> TransformsChangedSignal;
+  /// Signal which is fired whenever new transform data has arrived, from the thread the data arrived in
+  TransformsChangedSignal _transforms_changed_;
+
+
+  /************************* Internal Functions ****************************/
+
+  /** \brief An accessor to get a frame, which will throw an exception if the frame is no there.
+   * \param frame_number The frameID of the desired Reference Frame
+   *
+   * This is an internal function which will get the pointer to the frame associated with the frame id
+   * Possible Exception: tf::LookupException
+   */
+  TimeCacheInterfacePtr getFrame(CompactFrameID c_frame_id) const;
+
+  TimeCacheInterfacePtr allocateFrame(CompactFrameID cfid, bool is_static);
+
+
+  bool warnFrameId(const char* function_name_arg, const std::string& frame_id) const;
+  CompactFrameID validateFrameId(const char* function_name_arg, const std::string& frame_id) const;
+
+  /// String to number for frame lookup with dynamic allocation of new frames
+  CompactFrameID lookupFrameNumber(const std::string& frameid_str) const;
+
+  /// String to number for frame lookup with dynamic allocation of new frames
+  CompactFrameID lookupOrInsertFrameNumber(const std::string& frameid_str);
+
+  ///Number to string frame lookup may throw LookupException if number invalid
+  const std::string& lookupFrameString(CompactFrameID frame_id_num) const;
+
+  void createConnectivityErrorString(CompactFrameID source_frame, CompactFrameID target_frame, std::string* out) const;
+
+  /**@brief Return the latest rostime which is common across the spanning set
+   * zero if fails to cross */
+  int getLatestCommonTime(CompactFrameID target_frame, CompactFrameID source_frame, tf3::Time& time, std::string* error_string) const;
+
+  template<typename F>
+  int walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string) const;
+
+  /**@brief Traverse the transform tree. If frame_chain is not NULL, store the traversed frame tree in vector frame_chain.
+   * */
+  template<typename F>
+  int walkToTopParent(F& f, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
+
+  void testTransformableRequests();
+  bool canTransformInternal(CompactFrameID target_id, CompactFrameID source_id,
+                    const tf3::Time& time, std::string* error_msg) const;
+  bool canTransformNoLock(CompactFrameID target_id, CompactFrameID source_id,
+                      const tf3::Time& time, std::string* error_msg) const;
+
+
+  //Whether it is safe to use canTransform with a timeout. (If another thread is not provided it will always timeout.)
+  bool using_dedicated_thread_;
+
+public:
+  friend class TestBufferCore; // For unit testing
+
+};
+
+/** A helper class for testing internal APIs */
+class TestBufferCore
+{
+public:
+  int _walkToTopParent(BufferCore& buffer, tf3::Time time, CompactFrameID target_id, CompactFrameID source_id, std::string* error_string, std::vector<CompactFrameID> *frame_chain) const;
+  const std::string& _lookupFrameString(BufferCore& buffer, CompactFrameID frame_id_num) const
+  {
+    return buffer.lookupFrameString(frame_id_num);
+  }
+};
+}
+
+#endif //TF3_CORE_H

tf3-main/include/tf3/compat.h

@@ -0,0 +1,31 @@
+// Compatibility types to avoid using ros:: or geometry_msgs:: namespaces inside tf3
+#ifndef TF3_COMPAT_H
+#define TF3_COMPAT_H
+
+#include "tf3/time.h"
+#include <string>
+
+namespace tf3 {
+
+// Minimal header/message equivalents owned by tf3 (no ros:: or geometry_msgs::)
+struct HeaderMsg
+{
+  uint32_t seq = 0;
+  Time stamp;
+  std::string frame_id;
+};
+
+struct Vector3Msg { double x = 0, y = 0, z = 0; };
+struct QuaternionMsg { double x = 0, y = 0, z = 0, w = 1; };
+struct TransformMsg { Vector3Msg translation; QuaternionMsg rotation; };
+
+struct TransformStampedMsg
+{
+  HeaderMsg header;
+  std::string child_frame_id;
+  TransformMsg transform;
+};
+
+} // namespace tf3
+
+#endif // TF3_COMPAT_H

tf3-main/include/tf3/convert.h

@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2013, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#ifndef TF3_CONVERT_H
+#define TF3_CONVERT_H
+
+
+#include <tf3/transform_datatypes.h>
+#include <tf3/exceptions.h>
+#include <tf3/message_traits.h>
+#include <tf3/impl/convert.h>
+#include <tf3/compat.h>
+
+namespace tf3 {
+
+/**\brief The templated function expected to be able to do a transform
+ *
+ * This is the method which tf3 will use to try to apply a transform for any given datatype.   
+ * \param data_in The data to be transformed.
+ * \param data_out A reference to the output data.  Note this can point to data in and the method should be mutation safe.
+ * \param transform The transform to apply to data_in to fill data_out.  
+ * 
+ * This method needs to be implemented by client library developers
+ */
+template <class T>
+  void doTransform(const T& data_in, T& data_out, const tf3::TransformStampedMsg& transform);
+
+/**\brief Get the timestamp from data 
+ * \param t The data input.
+ * \return The timestamp associated with the data. The lifetime of the returned
+ * reference is bound to the lifetime of the argument.
+ */
+template <class T>
+  const tf3::Time& getTimestamp(const T& t);
+
+/**\brief Get the frame_id from data 
+ * \param t The data input.
+ * \return The frame_id associated with the data. The lifetime of the returned
+ * reference is bound to the lifetime of the argument.
+ */
+template <class T>
+  const std::string& getFrameId(const T& t);
+
+
+
+/* An implementation for Stamped<P> datatypes */
+template <class P>
+  const tf3::Time& getTimestamp(const tf3::Stamped<P>& t)
+  {
+    return t.stamp_;
+  }
+
+/* An implementation for Stamped<P> datatypes */
+template <class P>
+  const std::string& getFrameId(const tf3::Stamped<P>& t)
+  {
+    return t.frame_id_;
+  }
+
+/** Function that converts from one type to a ROS message type. It has to be
+ * implemented by each data type in tf3_* (except ROS messages) as it is
+ * used in the "convert" function.
+ * \param a an object of whatever type
+ * \return the conversion as a ROS message
+ */
+template<typename A, typename B>
+  B toMsg(const A& a);
+
+/** Function that converts from a ROS message type to another type. It has to be
+ * implemented by each data type in tf3_* (except ROS messages) as it is used
+ * in the "convert" function.
+ * \param a a ROS message to convert from
+ * \param b the object to convert to
+ */
+template<typename A, typename B>
+  void fromMsg(const A&, B& b);
+
+/** Function that converts any type to any type (messages or not).
+ * Matching toMsg and from Msg conversion functions need to exist.
+ * If they don't exist or do not apply (for example, if your two
+ * classes are ROS messages), just write a specialization of the function.
+ * \param a an object to convert from
+ * \param b the object to convert to
+ */
+template <class A, class B>
+  void convert(const A& a, B& b)
+  {
+    //printf("In double type convert\n");
+  impl::Converter<tf3::message_traits::IsMessage<A>::value, tf3::message_traits::IsMessage<B>::value>::convert(a, b);
+  }
+
+template <class A>
+  void convert(const A& a1, A& a2)
+  {
+    //printf("In single type convert\n");
+    if(&a1 != &a2)
+      a2 = a1;
+  }
+
+
+}
+
+#endif //TF3_CONVERT_H

tf3-main/include/tf3/exceptions.h

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#ifndef TF3_EXCEPTIONS_H
+#define TF3_EXCEPTIONS_H
+
+#include <stdexcept>
+
+namespace tf3{
+
+/** \brief A base class for all tf3 exceptions 
+ * This inherits from ros::exception 
+ * which inherits from std::runtime_exception
+ */
+class TransformException: public std::runtime_error
+{ 
+public:
+  TransformException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; };
+};
+
+
+  /** \brief An exception class to notify of no connection
+   * 
+   * This is an exception class to be thrown in the case 
+   * that the Reference Frame tree is not connected between
+   * the frames requested. */
+class ConnectivityException:public TransformException
+{ 
+public:
+  ConnectivityException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; };
+};
+
+
+/** \brief An exception class to notify of bad frame number 
+ * 
+ * This is an exception class to be thrown in the case that 
+ * a frame not in the graph has been attempted to be accessed.
+ * The most common reason for this is that the frame is not
+ * being published, or a parent frame was not set correctly 
+ * causing the tree to be broken.  
+ */
+class LookupException: public TransformException
+{ 
+public:
+  LookupException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; };
+};
+
+  /** \brief An exception class to notify that the requested value would have required extrapolation beyond current limits.
+   * 
+   */
+class ExtrapolationException: public TransformException 
+{ 
+public:
+  ExtrapolationException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; };
+};
+
+/** \brief An exception class to notify that one of the arguments is invalid
+ * 
+ * usually it's an uninitalized Quaternion (0,0,0,0)
+ * 
+ */
+class InvalidArgumentException: public TransformException  
+{ 
+public:
+  InvalidArgumentException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; };
+};
+
+/** \brief An exception class to notify that a timeout has occured
+ * 
+ * 
+ */
+class TimeoutException: public TransformException  
+{ 
+public:
+  TimeoutException(const std::string errorDescription) : tf3::TransformException(errorDescription) { ; };
+};
+
+
+}
+
+
+#endif //TF3_EXCEPTIONS_H

tf3-main/include/tf3/impl/convert.h

@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2013, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TF3_IMPL_CONVERT_H
+#define TF3_IMPL_CONVERT_H
+
+namespace tf3 {
+namespace impl {
+
+template <bool IS_MESSAGE_A, bool IS_MESSAGE_B>
+class Converter {
+public:
+  template<typename A, typename B>
+  static void convert(const A& a, B& b);
+};
+
+// The case where both A and B are messages should not happen: if you have two
+// messages that are interchangeable, well, that's against the ROS purpose:
+// only use one type. Worst comes to worst, specialize the original convert
+// function for your types.
+// if B == A, the templated version of convert with only one argument will be
+// used.
+//
+//template <>
+//template <typename A, typename B>
+//inline void Converter<true, true>::convert(const A& a, B& b);
+
+template <>
+template <typename A, typename B>
+inline void Converter<true, false>::convert(const A& a, B& b)
+{
+#ifdef _MSC_VER
+  tf3::fromMsg(a, b);
+#else
+  fromMsg(a, b);
+#endif
+}
+
+template <>
+template <typename A, typename B>
+inline void Converter<false, true>::convert(const A& a, B& b)
+{
+#ifdef _MSC_VER
+  b = tf3::toMsg(a);
+#else
+  b = toMsg(a);
+#endif
+}
+
+template <>
+template <typename A, typename B>
+inline void Converter<false, false>::convert(const A& a, B& b)
+{
+#ifdef _MSC_VER
+  tf3::fromMsg(tf3::toMsg(a), b);
+#else
+  fromMsg(toMsg(a), b);
+#endif
+}
+
+}
+}
+
+#endif //TF3_IMPL_CONVERT_H

tf3-main/include/tf3/impl/utils.h

@@ -0,0 +1,142 @@
+// Copyright 2014 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TF3_IMPL_UTILS_H
+#define TF3_IMPL_UTILS_H
+
+#include <tf3/transform_datatypes.h>
+#include <tf3/LinearMath/Quaternion.h>
+#include <tf3/compat.h>
+
+namespace tf3 {
+namespace impl {
+
+/** Function needed for the generalization of toQuaternion
+ * \param q a tf3::Quaternion
+ * \return a copy of the same quaternion
+ */
+inline
+tf3::Quaternion toQuaternion(const tf3::Quaternion& q) {
+    return q;
+  }
+
+/** Function needed for the generalization of toQuaternion
+ * \param q a tf3::QuaternionMsg wrapped in a stamped container
+ * \return a copy of the same quaternion as a tf3::Quaternion
+ */
+inline
+tf3::Quaternion toQuaternion(const tf3::QuaternionMsg& q) {
+    tf3::Quaternion res;
+    res.setValue(q.x, q.y, q.z, q.w);
+    return res;
+  }
+
+/** Function needed for the generalization of toQuaternion
+ * \param t some tf3::Stamped object
+ * \return a copy of the same quaternion as a tf3::Quaternion
+ */
+template<typename T>
+  tf3::Quaternion toQuaternion(const tf3::Stamped<T>& t) {
+    tf3::QuaternionMsg q = toMsg(t);
+    return toQuaternion(q);
+  }
+
+/** Generic version of toQuaternion. It tries to convert the argument
+ * to a geometry_msgs::Quaternion
+ * \param t some object
+ * \return a copy of the same quaternion as a tf3::Quaternion
+ */
+template<typename T>
+  tf3::Quaternion toQuaternion(const T& t) {
+    tf3::QuaternionMsg q = toMsg(t);
+    return toQuaternion(q);
+  }
+
+/** The code below is blantantly copied from urdfdom_headers
+ * only the normalization has been added.
+ * It computes the Euler roll, pitch yaw from a tf3::Quaternion
+ * It is equivalent to tf3::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+ * \param q a tf3::Quaternion
+ * \param yaw the computed yaw
+ * \param pitch the computed pitch
+ * \param roll the computed roll
+ */
+inline
+void getEulerYPR(const tf3::Quaternion& q, double &yaw, double &pitch, double &roll)
+{
+  double sqw;
+  double sqx;
+  double sqy;
+  double sqz;
+
+  sqx = q.x() * q.x();
+  sqy = q.y() * q.y();
+  sqz = q.z() * q.z();
+  sqw = q.w() * q.w();
+
+  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
+  double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
+  if (sarg <= -0.99999) {
+    pitch = -0.5*M_PI;
+    roll  = 0;
+    yaw   = -2 * atan2(q.y(), q.x());
+  } else if (sarg >= 0.99999) {
+    pitch = 0.5*M_PI;
+    roll  = 0;
+    yaw   = 2 * atan2(q.y(), q.x());
+  } else {
+    pitch = asin(sarg);
+    roll  = atan2(2 * (q.y()*q.z() + q.w()*q.x()), sqw - sqx - sqy + sqz);
+    yaw   = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
+  }
+}
+
+/** The code below is a simplified version of getEulerRPY that only
+ * returns the yaw. It is mostly useful in navigation where only yaw
+ * matters
+ * \param q a tf3::Quaternion
+ * \return the computed yaw
+ */
+inline
+double getYaw(const tf3::Quaternion& q)
+{
+  double yaw;
+
+  double sqw;
+  double sqx;
+  double sqy;
+  double sqz;
+
+  sqx = q.x() * q.x();
+  sqy = q.y() * q.y();
+  sqz = q.z() * q.z();
+  sqw = q.w() * q.w();
+
+  // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
+  double sarg = -2 * (q.x()*q.z() - q.w()*q.y()) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
+
+  if (sarg <= -0.99999) {
+    yaw   = -2 * atan2(q.y(), q.x());
+  } else if (sarg >= 0.99999) {
+    yaw   = 2 * atan2(q.y(), q.x());
+  } else {
+    yaw   = atan2(2 * (q.x()*q.y() + q.w()*q.z()), sqw + sqx - sqy - sqz);
+  }
+  return yaw;
+}
+
+}
+}
+
+#endif //TF3_IMPL_UTILS_H

tf3-main/include/tf3/macros.h

@@ -0,0 +1,13 @@
+// Minimal compatibility macros used by tf3 to avoid depending on ROS
+#ifndef TF3_MACROS_H_
+#define TF3_MACROS_H_
+
+#if defined(__clang__) || defined(__GNUC__)
+# define ROS_DEPRECATED __attribute__((deprecated))
+#elif defined(_MSC_VER)
+# define ROS_DEPRECATED __declspec(deprecated)
+#else
+# define ROS_DEPRECATED
+#endif
+
+#endif // TF3_MACROS_H_

tf3-main/include/tf3/message_traits.h

@@ -0,0 +1,12 @@
+// Minimal message_traits implementation for tf3 (no ROS dependency)
+#ifndef TF3_MESSAGE_TRAITS_H
+#define TF3_MESSAGE_TRAITS_H
+
+namespace tf3 {
+  namespace message_traits {
+    template <typename T>
+    struct IsMessage { static const bool value = false; };
+  }
+}
+
+#endif // TF3_MESSAGE_TRAITS_H

tf3-main/include/tf3/tf3_c_api.h

@@ -0,0 +1,133 @@
+/*
+ * tf3_c_api.h
+ * C API wrapper for tf3 BufferCore to enable P/Invoke from C#
+ *
+ * Based on libtf2 tf2_c_api.
+ */
+
+#ifndef TF3_C_API_H
+#define TF3_C_API_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/* Opaque handle for BufferCore */
+typedef void* TF3_BufferCore;
+
+/* Transform structure compatible with C# */
+typedef struct {
+    /* Header */
+    int64_t timestamp_sec;
+    int64_t timestamp_nsec;
+    char frame_id[256];
+    char child_frame_id[256];
+    
+    /* Translation */
+    double translation_x;
+    double translation_y;
+    double translation_z;
+    
+    /* Rotation (quaternion) */
+    double rotation_x;
+    double rotation_y;
+    double rotation_z;
+    double rotation_w;
+} TF3_Transform;
+
+/* Error codes */
+typedef enum {
+    TF3_OK = 0,
+    TF3_ERROR_LOOKUP = 1,
+    TF3_ERROR_CONNECTIVITY = 2,
+    TF3_ERROR_EXTRAPOLATION = 3,
+    TF3_ERROR_INVALID_ARGUMENT = 4,
+    TF3_ERROR_TIMEOUT = 5,
+    TF3_ERROR_UNKNOWN = 99
+} TF3_ErrorCode;
+
+/* Export macro for shared library */
+#ifdef _WIN32
+    #ifdef TF3_C_API_EXPORTS
+        #define TF3_C_API __declspec(dllexport)
+    #else
+        #define TF3_C_API __declspec(dllimport)
+    #endif
+#else
+    #define TF3_C_API __attribute__((visibility("default")))
+#endif
+
+TF3_C_API TF3_BufferCore tf3_buffer_create(int32_t cache_time_sec);
+TF3_C_API void tf3_buffer_destroy(TF3_BufferCore buffer);
+
+TF3_C_API bool tf3_set_transform(
+    TF3_BufferCore buffer,
+    const TF3_Transform* transform,
+    const char* authority,
+    bool is_static
+);
+
+TF3_C_API bool tf3_lookup_transform(
+    TF3_BufferCore buffer,
+    const char* target_frame,
+    const char* source_frame,
+    int64_t time_sec,
+    int64_t time_nsec,
+    TF3_Transform* transform,
+    TF3_ErrorCode* error_code
+);
+
+TF3_C_API bool tf3_lookup_transform_full(
+    TF3_BufferCore buffer,
+    const char* target_frame,
+    int64_t target_time_sec,
+    int64_t target_time_nsec,
+    const char* source_frame,
+    int64_t source_time_sec,
+    int64_t source_time_nsec,
+    const char* fixed_frame,
+    TF3_Transform* transform,
+    TF3_ErrorCode* error_code
+);
+
+TF3_C_API bool tf3_can_transform(
+    TF3_BufferCore buffer,
+    const char* target_frame,
+    const char* source_frame,
+    int64_t time_sec,
+    int64_t time_nsec,
+    char* error_msg,
+    int32_t error_msg_len
+);
+
+TF3_C_API int32_t tf3_get_all_frame_names(
+    TF3_BufferCore buffer,
+    char* frames,
+    int32_t frames_len
+);
+
+TF3_C_API bool tf3_get_frame_tree(
+    TF3_BufferCore buffer,
+    char* output,
+    int32_t output_len
+);
+
+TF3_C_API void tf3_clear(TF3_BufferCore buffer);
+TF3_C_API void tf3_get_current_time(int64_t* sec, int64_t* nsec);
+
+TF3_C_API bool tf3_get_last_error(
+    TF3_BufferCore buffer,
+    char* error_msg,
+    int32_t error_msg_len
+);
+
+TF3_C_API const char* tf3_get_version(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* TF3_C_API_H */

tf3-main/include/tf3/time.h

@@ -0,0 +1,152 @@
+// Minimal tf3::Time and tf3::Duration (non-ROS) for standalone tf3
+#ifndef TF3_TIME_H
+#define TF3_TIME_H
+
+#include <cstdint>
+#include <limits>
+#include <cmath>
+#include <chrono>
+#include <thread>
+
+namespace tf3 {
+
+class Duration;
+
+class Time {
+public:
+  uint32_t sec;
+  uint32_t nsec;
+
+  Time(): sec(0), nsec(0) {}
+  Time(uint32_t s, uint32_t ns): sec(s), nsec(ns) {}
+
+  static Time now()
+  {
+    using namespace std::chrono;
+    auto now = system_clock::now().time_since_epoch();
+    auto ns = duration_cast<nanoseconds>(now).count();
+    Time t;
+    t.sec = static_cast<uint32_t>(ns / 1000000000ULL);
+    t.nsec = static_cast<uint32_t>(ns % 1000000000ULL);
+    return t;
+  }
+
+  static Time fromSec(double s)
+  {
+    Time t;
+    t.sec = static_cast<uint32_t>(std::floor(s));
+    t.nsec = static_cast<uint32_t>((s - t.sec) * 1e9);
+    return t;
+  }
+
+  double toSec() const { return static_cast<double>(sec) + static_cast<double>(nsec) * 1e-9; }
+  bool isZero() const { return sec == 0 && nsec == 0; }
+
+  bool operator==(const Time& o) const { return sec == o.sec && nsec == o.nsec; }
+  bool operator!=(const Time& o) const { return !(*this == o); }
+  bool operator<(const Time& o) const { return sec < o.sec || (sec == o.sec && nsec < o.nsec); }
+  bool operator>(const Time& o) const { return o < *this; }
+  bool operator<=(const Time& o) const { return !(*this > o); }
+  bool operator>=(const Time& o) const { return !(*this < o); }
+
+  friend Duration operator-(const Time& a, const Time& b);
+  friend Time operator+(const Time& t, const Duration& d);
+  friend Time operator-(const Time& t, const Duration& d);
+
+  static const Time& TIME_MAX()
+  {
+    static Time tm{std::numeric_limits<uint32_t>::max(), std::numeric_limits<uint32_t>::max()};
+    return tm;
+  }
+};
+
+class Duration {
+public:
+  int32_t sec;
+  int32_t nsec;
+
+  Duration(): sec(0), nsec(0) {}
+  Duration(int32_t s, int32_t ns): sec(s), nsec(ns) { normalize(); }
+  explicit Duration(double seconds)
+  {
+    sec = static_cast<int32_t>(std::floor(seconds));
+    nsec = static_cast<int32_t>((seconds - sec) * 1e9);
+    normalize();
+  }
+
+  static Duration fromSec(double s) { return Duration(s); }
+  static Duration fromNSec(int64_t ns_total)
+  {
+    int32_t s = static_cast<int32_t>(ns_total / 1000000000LL);
+    int32_t ns = static_cast<int32_t>(ns_total % 1000000000LL);
+    return Duration(s, ns);
+  }
+
+  double toSec() const { return static_cast<double>(sec) + static_cast<double>(nsec) * 1e-9; }
+
+  void normalize()
+  {
+    if (nsec >= 1000000000L)
+    {
+      sec += nsec / 1000000000L;
+      nsec = nsec % 1000000000L;
+    }
+    else if (nsec < 0)
+    {
+      int32_t borrow = (std::abs(nsec) / 1000000000L) + 1;
+      sec -= borrow;
+      nsec += borrow * 1000000000L;
+    }
+  }
+
+  Duration operator+(const Duration& other) const { return Duration(sec + other.sec, nsec + other.nsec); }
+  Duration operator-(const Duration& other) const { return Duration(sec - other.sec, nsec - other.nsec); }
+  Duration& operator+=(const Duration& other) { sec += other.sec; nsec += other.nsec; normalize(); return *this; }
+  Duration& operator-=(const Duration& other) { sec -= other.sec; nsec -= other.nsec; normalize(); return *this; }
+
+  bool operator==(const Duration& o) const { return sec == o.sec && nsec == o.nsec; }
+  bool operator!=(const Duration& o) const { return !(*this == o); }
+  bool operator<(const Duration& o) const { return sec < o.sec || (sec == o.sec && nsec < o.nsec); }
+  bool operator>(const Duration& o) const { return o < *this; }
+  bool operator<=(const Duration& o) const { return !(*this > o); }
+  bool operator>=(const Duration& o) const { return !(*this < o); }
+
+  void sleep() const { if (sec < 0 || nsec < 0) return; auto ns_total = static_cast<int64_t>(sec) * 1000000000LL + nsec; std::this_thread::sleep_for(std::chrono::nanoseconds(ns_total)); }
+};
+
+inline Duration operator-(const Time& a, const Time& b)
+{
+  int32_t s = static_cast<int32_t>(a.sec) - static_cast<int32_t>(b.sec);
+  int32_t ns = static_cast<int32_t>(a.nsec) - static_cast<int32_t>(b.nsec);
+  Duration d(s, ns);
+  d.normalize();
+  return d;
+}
+
+inline Time operator+(const Time& t, const Duration& d)
+{
+  int64_t total_ns = static_cast<int64_t>(t.sec) * 1000000000LL + t.nsec + static_cast<int64_t>(d.sec) * 1000000000LL + d.nsec;
+  Time out;
+  out.sec = static_cast<uint32_t>(total_ns / 1000000000LL);
+  out.nsec = static_cast<uint32_t>(total_ns % 1000000000LL);
+  return out;
+}
+
+inline Time operator-(const Time& t, const Duration& d)
+{
+  int64_t total_ns = static_cast<int64_t>(t.sec) * 1000000000LL + t.nsec - (static_cast<int64_t>(d.sec) * 1000000000LL + d.nsec);
+  if (total_ns < 0) total_ns = 0;
+  Time out;
+  out.sec = static_cast<uint32_t>(total_ns / 1000000000LL);
+  out.nsec = static_cast<uint32_t>(total_ns % 1000000000LL);
+  return out;
+}
+
+// Provide tf3::TIME_MAX symbol
+namespace {
+  static const Time TIME_MAX = Time::TIME_MAX();
+}
+
+} // namespace tf3
+
+#endif // TF3_TIME_H

tf3-main/include/tf3/time_cache.h

@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#ifndef TF3_TIME_CACHE_H
+#define TF3_TIME_CACHE_H
+
+#include "transform_storage.h"
+
+#include <deque>
+
+#include "tf3/time.h"
+
+#include <boost/shared_ptr.hpp>
+
+namespace tf3
+{
+
+typedef std::pair<tf3::Time, CompactFrameID> P_TimeAndFrameID;
+
+class TimeCacheInterface
+{
+public:
+  /** \brief Access data from the cache */
+  virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0)=0; //returns false if data unavailable (should be thrown as lookup exception
+
+  /** \brief Insert data into the cache */
+  virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0)=0;
+
+  /** @brief Clear the list of stored values */
+  virtual void clearList()=0;
+
+  /** \brief Retrieve the parent at a specific time */
+  virtual CompactFrameID getParent(tf3::Time time, std::string* error_str) = 0;
+
+  /**
+   * \brief Get the latest time stored in this cache, and the parent associated with it.  Returns parent = 0 if no data.
+   */
+  virtual P_TimeAndFrameID getLatestTimeAndParent() = 0;
+
+
+  /// Debugging information methods
+  /** @brief Get the length of the stored list */
+  virtual unsigned int getListLength()=0;
+
+  /** @brief Get the latest timestamp cached */
+  virtual tf3::Time getLatestTimestamp()=0;
+
+  /** @brief Get the oldest timestamp cached */
+  virtual tf3::Time getOldestTimestamp()=0;
+};
+
+typedef boost::shared_ptr<TimeCacheInterface> TimeCacheInterfacePtr;
+
+/** \brief A class to keep a sorted linked list in time
+ * This builds and maintains a list of timestamped
+ * data.  And provides lookup functions to get
+ * data out as a function of time. */
+class TimeCache : public TimeCacheInterface
+{
+ public:
+  static const int MIN_INTERPOLATION_DISTANCE = 5; //!< Number of nano-seconds to not interpolate below.
+  static const unsigned int MAX_LENGTH_LINKED_LIST = 1000000; //!< Maximum length of linked list, to make sure not to be able to use unlimited memory.
+  static const int64_t DEFAULT_MAX_STORAGE_TIME = 10ULL * 1000000000LL; //!< default value of 10 seconds storage
+
+  TimeCache(tf3::Duration  max_storage_time = tf3::Duration::fromNSec(DEFAULT_MAX_STORAGE_TIME));
+
+
+  /// Virtual methods
+
+  virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0);
+  virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0);
+  virtual void clearList();
+  virtual CompactFrameID getParent(tf3::Time time, std::string* error_str);
+  virtual P_TimeAndFrameID getLatestTimeAndParent();
+
+  /// Debugging information methods
+  virtual unsigned int getListLength();
+  virtual tf3::Time getLatestTimestamp();
+  virtual tf3::Time getOldestTimestamp();
+  
+
+private:
+  typedef std::deque<TransformStorage> L_TransformStorage;
+  L_TransformStorage storage_;
+
+  tf3::Duration max_storage_time_;
+
+
+  /// A helper function for getData
+  //Assumes storage is already locked for it
+
+  inline uint8_t findClosest(TransformStorage*& one, TransformStorage*& two, tf3::Time target_time, std::string* error_str);
+
+  inline void interpolate(const TransformStorage& one, const TransformStorage& two, tf3::Time time, TransformStorage& output);
+
+
+  void pruneList();
+
+
+
+};
+
+class StaticCache : public TimeCacheInterface
+{
+ public:
+  /// Virtual methods
+
+  virtual bool getData(tf3::Time time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception
+  virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0);
+  virtual void clearList();
+  virtual CompactFrameID getParent(tf3::Time time, std::string* error_str);
+  virtual P_TimeAndFrameID getLatestTimeAndParent();
+
+
+  /// Debugging information methods
+  virtual unsigned int getListLength();
+  virtual tf3::Time getLatestTimestamp();
+  virtual tf3::Time getOldestTimestamp();
+  
+
+private:
+  TransformStorage  storage_;
+};
+
+}
+
+#endif // TF3_TIME_CACHE_H

tf3-main/include/tf3/transform_datatypes.h

@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#ifndef TF3_TRANSFORM_DATATYPES_H
+#define TF3_TRANSFORM_DATATYPES_H
+
+#include <string>
+#include "tf3/time.h"
+
+namespace tf3
+{
+
+/** \brief The data type which will be cross compatable with geometry_msgs
+ * This is the tf3 datatype equivilant of a MessageStamped */
+template <typename T>
+class Stamped : public T{
+ public:
+  tf3::Time stamp_; ///< The timestamp associated with this data
+  std::string frame_id_; ///< The frame_id associated this data
+
+  /** Default constructor */
+  Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation
+
+  /** Full constructor */
+  Stamped(const T& input, const tf3::Time& timestamp, const std::string & frame_id) :
+    T (input), stamp_ ( timestamp ), frame_id_ (frame_id){ } ;
+  
+  /** Copy Constructor */
+  Stamped(const Stamped<T>& s):
+    T (s),
+    stamp_(s.stamp_),
+    frame_id_(s.frame_id_) {}
+
+  /** Copy assignment operator */
+  Stamped<T> & operator=(const Stamped<T> & rhs)
+  {
+    T::operator=(rhs);
+    stamp_ = rhs.stamp_;
+    frame_id_ = rhs.frame_id_;
+    return *this;
+  }
+
+  
+  /** Set the data element */
+  void setData(const T& input){*static_cast<T*>(this) = input;};
+};
+
+/** \brief Comparison Operator for Stamped datatypes */
+template <typename T> 
+bool operator==(const Stamped<T> &a, const Stamped<T> &b) {
+  return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast<const T&>(a) == static_cast<const T&>(b);
+}
+
+
+}
+#endif //TF3_TRANSFORM_DATATYPES_H

tf3-main/include/tf3/transform_storage.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2010, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#ifndef TF3_TRANSFORM_STORAGE_H
+#define TF3_TRANSFORM_STORAGE_H
+
+#include <tf3/LinearMath/Vector3.h>
+#include <tf3/LinearMath/Quaternion.h>
+
+
+#include "tf3/compat.h"
+
+namespace tf3
+{
+
+typedef uint32_t CompactFrameID;
+
+/** \brief Storage for transforms and their parent */
+class TransformStorage
+{
+public:
+  TransformStorage();
+  TransformStorage(const tf3::TransformStampedMsg& data, CompactFrameID frame_id, CompactFrameID child_frame_id);
+
+  TransformStorage(const TransformStorage& rhs)
+  {
+    *this = rhs;
+  }
+
+  TransformStorage& operator=(const TransformStorage& rhs)
+  {
+#if 01
+    rotation_ = rhs.rotation_;
+    translation_ = rhs.translation_;
+    stamp_ = rhs.stamp_;
+    frame_id_ = rhs.frame_id_;
+    child_frame_id_ = rhs.child_frame_id_;
+#endif
+    return *this;
+  }
+
+  tf3::Quaternion rotation_;
+  tf3::Vector3 translation_;
+  tf3::Time stamp_;
+  CompactFrameID frame_id_;
+  CompactFrameID child_frame_id_;
+};
+
+}
+
+#endif // TF3_TRANSFORM_STORAGE_H
+

tf3-main/include/tf3/utils.h

@@ -0,0 +1,66 @@
+// Copyright 2014 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TF3_UTILS_H
+#define TF3_UTILS_H
+
+#include <tf3/LinearMath/Transform.h>
+#include <tf3/LinearMath/Quaternion.h>
+#include <tf3/impl/utils.h>
+
+namespace tf3 {
+/** Return the yaw, pitch, roll of anything that can be converted to a tf3::Quaternion
+ * The conventions are the usual ROS ones defined in tf3/LineMath/Matrix3x3.h
+ * \param a the object to get data from (it represents a rotation/quaternion)
+ * \param yaw yaw
+ * \param pitch pitch
+ * \param roll roll
+ */
+template <class A>
+  void getEulerYPR(const A& a, double& yaw, double& pitch, double& roll)
+  {
+    tf3::Quaternion q = impl::toQuaternion(a);
+    impl::getEulerYPR(q, yaw, pitch, roll);
+  }
+
+/** Return the yaw of anything that can be converted to a tf3::Quaternion
+ * The conventions are the usual ROS ones defined in tf3/LineMath/Matrix3x3.h
+ * This function is a specialization of getEulerYPR and is useful for its
+ * wide-spread use in navigation
+ * \param a the object to get data from (it represents a rotation/quaternion)
+ * \param yaw yaw
+ */
+template <class A>
+  double getYaw(const A& a)
+  {
+    tf3::Quaternion q = impl::toQuaternion(a);
+    return impl::getYaw(q);
+  }
+
+/** Return the identity for any type that can be converted to a tf3::Transform
+ * \return an object of class A that is an identity transform
+ */
+template <class A>
+  A getTransformIdentity()
+  {
+    tf3::Transform t;
+    t.setIdentity();
+    A a;
+    convert(t, a);
+    return a;
+  }
+
+}
+
+#endif //TF3_UTILS_H