This section of the archives stores flipcode's complete Developer Toolbox collection, featuring a variety of mini-articles and source code contributions from our readers.

| Right-handed Math Library Submitted by

Here is the math library that I have been using in my OpenGL engine. The math library is suitable for right-handed graphics APIs (such as OpenGL). The source code is fully documented. I have included a somewhat incomplete tester with the math library. The tester is intended to both test out the math library for correctness and to demonstrate how to use the math library. The code was written using VC 6 If you find any bugs in the library, please report them to me at dhpo@msn.com Enjoy.
David Poon.

Currently browsing [mathlib.zip] (51,635 bytes) - [mathlib/mathlib.cpp] - (31,825 bytes)

    //------------------------------------------------------------------------
    // Math Library for right-handed 3D graphics APIs.
    // Copyright (C) 2001-2002 David Poon
    //
    // This library is free software; you can redistribute it and/or
    // modify it under the terms of the GNU Lesser General Public
    // License as published by the Free Software Foundation; either
    // version 2.1 of the License, or (at your option) any later version.
    //
    // This library is distributed in the hope that it will be useful,
    // but WITHOUT ANY WARRANTY; without even the implied warranty of
    // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    // GNU Lesser General Public License for more details.
    //
    // You should have received a copy of the GNU Lesser General
    // Public License along with this library; if not, write to the
    // Free Software Foundation, Inc., 59 Temple Place, Suite 330,
    // Boston, MA 02111-1307 USA
    //------------------------------------------------------------------------
    // Revision History:
    //
    // 31/10/2001 David Poon
    // - Changed Vector::angle() to return the angle in degrees NOT radians.
    //
    // 06/11/2001 David Poon
    // - Added Quaternion::set() method.
    // - Rewrote Quaternion::toRotationMatrix() method.
    // - Changed value of PI to match the M_PI definition in GCC math.h
    //
    // 10/11/2001 David Poon
    // - Removed Camera class from the math library.
    //------------------------------------------------------------------------

    #include "mathlib.hpp"
    #include <cassert>

    // PI - matches definition in GCC's math.h
    const float PI = 3.14159265358979323846f;

    //------------------------------------------------------------------------
    // class Matrix
    //------------------------------------------------------------------------
    // References:
    // 1. Tomas Mollar and Eric Haines, "Real-Time Rendering," A K Peters
    //    Ltd., Massachusetts, 1999.
    //
    // 2. Paul Nettle's Matrix template class. http://www.fluidstudios.com
    //
    // 3. Tomas Moller and John F. Hughes, "Efficiently building a matrix
    //    to rotate one vector to another," Journal of Graphics Tools,
    //    4(4):1-4, 1999.
    //------------------------------------------------------------------------

    Matrix::Matrix() {}

    Matrix::~Matrix() {}

    Matrix::Matrix(const Matrix &rhs)
    {
    	for (int i = 0; i < 16; i++)
    		m_mtx[i] = rhs.m_mtx[i];
    }

    Matrix &Matrix::operator=(const Matrix &rhs)
    {
    	if (this != &rhs)
    	{
    		for (int i = 0; i < 16; i++)
    			m_mtx[i] = rhs.m_mtx[i];
    	}

    	return *this;
    }

    bool Matrix::operator==(const Matrix &rhs) const
    {
    	for (int i = 0; i < 16; i++)
    	{
    		if (!CloseEnough(m_mtx[i], rhs.m_mtx[i]))
    			return false;
    	}

    	return true;
    }

    bool Matrix::operator!=(const Matrix &rhs) const
    {
    	return !(*this == rhs);
    }

    void Matrix::add(const Matrix &m)
    {
    	for (int i = 0; i < 16; i++)
    		m_mtx[i] += m.m_mtx[i];
    }

    void Matrix::add(const Matrix &m1, const Matrix &m2)
    {
    	Matrix tmp(m1);
    	tmp.add(m2);
    	*this = tmp;
    }

    void Matrix::subtract(const Matrix &m)
    {
    	for (int i = 0; i < 16; i++)
    		m_mtx[i] -= m.m_mtx[i];
    }

    void Matrix::subtract(const Matrix &m1, const Matrix &m2)
    {
    	Matrix tmp(m1);
    	tmp.subtract(m2);
    	*this = tmp;
    }

    void Matrix::multiply(float scalar)
    {
    	for (int i = 0; i < 4; i++)
    	{
    		for (int j = 0; j < 4; j++)
    			elementAt(i,j) *= scalar;
    	}
    }

    void Matrix::multiply(const Matrix &m)
    {
    	Matrix temp;

    	for (int i = 0; i < 4; i++)
    	{
    		for (int j = 0; j < 4; j++)
    		{
    			temp.elementAt(i, j) = 0.f;

    			for (int k = 0; k < 4; k++)
    				temp.elementAt(i, j) += elementAt(i, k) * m.elementAt(k, j);
    		}
    	}

    	*this = temp;
    }

    void Matrix::multiply(const Matrix &m1, const Matrix &m2)
    {
    	Matrix tmp(m1);
    	tmp.multiply(m2);
    	*this = tmp;
    }

    void Matrix::loadIdentity()
    {
    	// Loads 'this' Matrix with the Identity Matrix.
    	//
    	//     | 1	0  0  0 |
    	// I = | 0	1  0  0 |
    	//     | 0  0  1  0 |
    	//     | 0  0  0  1 |
    	//

    	m_mtx[0] = 1.f;
    	m_mtx[1] = 0.f;
    	m_mtx[2] = 0.f;
    	m_mtx[3] = 0.f;

    	m_mtx[4] = 0.f;
    	m_mtx[5] = 1.f;
    	m_mtx[6] = 0.f;
    	m_mtx[7] = 0.f;

    	m_mtx[8] = 0.f;
    	m_mtx[9] = 0.f;
    	m_mtx[10] = 1.f;
    	m_mtx[11] = 0.f;

    	m_mtx[12] = 0.f;
    	m_mtx[13] = 0.f;
    	m_mtx[14] = 0.f;
    	m_mtx[15] = 1.f;
    }

    bool Matrix::inverse(const Matrix &m)
    {
    	// Calculates the inverse of Matrix 'm' and stores the
    	// result in 'this' Matrix. The inverse is calculated using
    	// Cramer's rule. If no inverse exists then the Identity Matrix is
    	// loaded into 'this' Matrix and 'false' is returned.

    	float d = (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) * (m(2, 2) * m(3, 3) - m(3, 2) * m(2, 3))	- (m(0, 0) * m(2, 1) - m(2, 0) * m(0, 1)) * (m(1, 2) * m(3, 3) - m(3, 2) * m(1, 3))
    			+ (m(0, 0) * m(3, 1) - m(3, 0) * m(0, 1)) * (m(1, 2) * m(2, 3) - m(2, 2) * m(1, 3))	+ (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)) * (m(0, 2) * m(3, 3) - m(3, 2) * m(0, 3))
    			- (m(1, 0) * m(3, 1) - m(3, 0) * m(1, 1)) * (m(0, 2) * m(2, 3) - m(2, 2) * m(0, 3))	+ (m(2, 0) * m(3, 1) - m(3, 0) * m(2, 1)) * (m(0, 2) * m(1, 3) - m(1, 2) * m(0, 3));

    	if (CloseEnough(d, 0.f))
    	{
    		loadIdentity();
    		return false;
    	}
    	else
    	{
    		d = 1.f / d;

    		Matrix temp;

    		temp(0, 0) = d * (m(1, 1) * (m(2, 2) * m(3, 3) - m(3, 2) * m(2, 3)) + m(2, 1) * (m(3, 2) * m(1, 3) - m(1, 2) * m(3, 3)) + m(3, 1) * (m(1, 2) * m(2, 3) - m(2, 2) * m(1, 3)));
    		temp(1, 0) = d * (m(1, 2) * (m(2, 0) * m(3, 3) - m(3, 0) * m(2, 3)) + m(2, 2) * (m(3, 0) * m(1, 3) - m(1, 0) * m(3, 3)) + m(3, 2) * (m(1, 0) * m(2, 3) - m(2, 0) * m(1, 3)));
    		temp(2, 0) = d * (m(1, 3) * (m(2, 0) * m(3, 1) - m(3, 0) * m(2, 1)) + m(2, 3) * (m(3, 0) * m(1, 1) - m(1, 0) * m(3, 1)) + m(3, 3) * (m(1, 0) * m(2, 1) - m(2, 0) * m(1, 1)));
    		temp(3, 0) = d * (m(1, 0) * (m(3, 1) * m(2, 2) - m(2, 1) * m(3, 2)) + m(2, 0) * (m(1, 1) * m(3, 2) - m(3, 1) * m(1, 2)) + m(3, 0) * (m(2, 1) * m(1, 2) - m(1, 1) * m(2, 2)));
    		temp(0, 1) = d * (m(2, 1) * (m(0, 2) * m(3, 3) - m(3, 2) * m(0, 3)) + m(3, 1) * (m(2, 2) * m(0, 3) - m(0, 2) * m(2, 3)) + m(0, 1) * (m(3, 2) * m(2, 3) - m(2, 2) * m(3, 3)));
    		temp(1, 1) = d * (m(2, 2) * (m(0, 0) * m(3, 3) - m(3, 0) * m(0, 3)) + m(3, 2) * (m(2, 0) * m(0, 3) - m(0, 0) * m(2, 3)) + m(0, 2) * (m(3, 0) * m(2, 3) - m(2, 0) * m(3, 3)));
    		temp(2, 1) = d * (m(2, 3) * (m(0, 0) * m(3, 1) - m(3, 0) * m(0, 1)) + m(3, 3) * (m(2, 0) * m(0, 1) - m(0, 0) * m(2, 1)) + m(0, 3) * (m(3, 0) * m(2, 1) - m(2, 0) * m(3, 1)));
    		temp(3, 1) = d * (m(2, 0) * (m(3, 1) * m(0, 2) - m(0, 1) * m(3, 2)) + m(3, 0) * (m(0, 1) * m(2, 2) - m(2, 1) * m(0, 2)) + m(0, 0) * (m(2, 1) * m(3, 2) - m(3, 1) * m(2, 2)));
    		temp(0, 2) = d * (m(3, 1) * (m(0, 2) * m(1, 3) - m(1, 2) * m(0, 3)) + m(0, 1) * (m(1, 2) * m(3, 3) - m(3, 2) * m(1, 3)) + m(1, 1) * (m(3, 2) * m(0, 3) - m(0, 2) * m(3, 3)));
    		temp(1, 2) = d * (m(3, 2) * (m(0, 0) * m(1, 3) - m(1, 0) * m(0, 3)) + m(0, 2) * (m(1, 0) * m(3, 3) - m(3, 0) * m(1, 3)) + m(1, 2) * (m(3, 0) * m(0, 3) - m(0, 0) * m(3, 3)));
    		temp(2, 2) = d * (m(3, 3) * (m(0, 0) * m(1, 1) - m(1, 0) * m(0, 1)) + m(0, 3) * (m(1, 0) * m(3, 1) - m(3, 0) * m(1, 1)) + m(1, 3) * (m(3, 0) * m(0, 1) - m(0, 0) * m(3, 1)));
    		temp(3, 2) = d * (m(3, 0) * (m(1, 1) * m(0, 2) - m(0, 1) * m(1, 2)) + m(0, 0) * (m(3, 1) * m(1, 2) - m(1, 1) * m(3, 2)) + m(1, 0) * (m(0, 1) * m(3, 2) - m(3, 1) * m(0, 2)));
    		temp(0, 3) = d * (m(0, 1) * (m(2, 2) * m(1, 3) - m(1, 2) * m(2, 3)) + m(1, 1) * (m(0, 2) * m(2, 3) - m(2, 2) * m(0, 3)) + m(2, 1) * (m(1, 2) * m(0, 3) - m(0, 2) * m(1, 3)));
    		temp(1, 3) = d * (m(0, 2) * (m(2, 0) * m(1, 3) - m(1, 0) * m(2, 3)) + m(1, 2) * (m(0, 0) * m(2, 3) - m(2, 0) * m(0, 3)) + m(2, 2) * (m(1, 0) * m(0, 3) - m(0, 0) * m(1, 3)));
    		temp(2, 3) = d * (m(0, 3) * (m(2, 0) * m(1, 1) - m(1, 0) * m(2, 1)) + m(1, 3) * (m(0, 0) * m(2, 1) - m(2, 0) * m(0, 1)) + m(2, 3) * (m(1, 0) * m(0, 1) - m(0, 0) * m(1, 1)));
    		temp(3, 3) = d * (m(0, 0) * (m(1, 1) * m(2, 2) - m(2, 1) * m(1, 2)) + m(1, 0) * (m(2, 1) * m(0, 2) - m(0, 1) * m(2, 2)) + m(2, 0) * (m(0, 1) * m(1, 2) - m(1, 1) * m(0, 2)));

    		*this = temp;
    		return true;
    	}
    }

    void Matrix::transpose(const Matrix &m)
    {
    	// Calculates the transpose of Matrix 'm' and stores the transposed
    	// Matrix in 'this' Matrix.

    	Matrix temp;

    	for (int i = 0; i < 4; i++)
    	{
    		for (int j = 0; j < 4; j++)
    			temp(i, j) = m(j, i);
    	}

    	*this = temp;
    }

    void Matrix::translate(float tx, float ty, float tz)
    {
    	// Creates a translation matrix and stores it in 'this' matrix.
    	//
    	//                 | 1  0  0  tx |
    	// t(tx, ty, tz) = | 0  1  0  ty |
    	//                 | 0  0  1  tz |
    	//                 | 0  0  0  1  |

    	elementAt(0, 0) = 1.f;
    	elementAt(0, 1) = 0.f;
    	elementAt(0, 2) = 0.f;
    	elementAt(0, 3) = tx;

    	elementAt(1, 0) = 0.f;
    	elementAt(1, 1) = 1.f;
    	elementAt(1, 2) = 0.f;
    	elementAt(1, 3) = ty;

    	elementAt(2, 0) = 0.f;
    	elementAt(2, 1) = 0.f;
    	elementAt(2, 2) = 1.f;
    	elementAt(2, 3) = tz;

    	elementAt(3, 0) = 0.f;
    	elementAt(3, 1) = 0.f;
    	elementAt(3, 2) = 0.f;
    	elementAt(3, 3) = 1.f;
    }

    void Matrix::scale(float sx, float sy, float sz)
    {
    	// Creates a scaling matrix and stores it in 'this' matrix.
    	//
    	//                 | sx   0    0    0 |
    	// S(sx, sy, sz) = | 0    sy   0    0 |
    	//                 | 0    0    sz   0 |
    	//                 | 0    0    0    1 |
    	//

    	elementAt(0, 0) = sx;
    	elementAt(0, 1) = 0.f;
    	elementAt(0, 2) = 0.f;
    	elementAt(0, 3) = 0.f;

    	elementAt(1, 0) = 0.f;
    	elementAt(1, 1) = sy;
    	elementAt(1, 2) = 0.f;
    	elementAt(1, 3) = 0.f;

    	elementAt(2, 0) = 0.f;
    	elementAt(2, 1) = 0.f;
    	elementAt(2, 2) = sz;
    	elementAt(2, 3) = 0.f;

    	elementAt(3, 0) = 0.f;
    	elementAt(3, 1) = 0.f;
    	elementAt(3, 2) = 0.f;
    	elementAt(3, 3) = 1.f;
    }

    void Matrix::rotate(float angle, Vector &axis)
    {
    	// Creates a rotation matrix about the axis (x, y, z). The axis must
    	// be a unit vector. If it isn't rotate() will normalize the axis.
    	// The angle is assumed to be in degrees.
    	//
    	// Let u = axis of rotation = (x, y, z)
    	//
    	//             | x^2(1-c) + c   xy(1-c) - zs   xz(1-c) + ys   0 |
    	// Ru(angle) = | yx(1-c) + zs   y^2(1-c) + c   yz(1-c) - xs   0 |
    	//             | xz(1-c) - ys   yz(1-c) + xs   z^2(1-c) + c   0 |
    	//             |      0              0               0        1 |
    	//
    	// where,
    	//	c = cos(angle)
    	//  s = sin(angle)
    	//

    	if (CloseEnough(axis.length(), 0.f))
    	{
    		loadIdentity();
    	}
    	else
    	{
    		axis.normalize();
    		float x = axis.x;
    		float y = axis.y;
    		float z = axis.z;

    		angle = Degrees2Radians(angle);
    		float c = cosf(angle);
    		float s = sinf(angle);

    		elementAt(0, 0) = x * x * (1.f - c) + c;
    		elementAt(0, 1) = x * y * (1.f - c) - (z * s);
    		elementAt(0, 2) = x * z * (1.f - c) + (y * s);
    		elementAt(0, 3) = 0.f;

    		elementAt(1, 0) = y * x * (1.f - c) + (z * s);
    		elementAt(1, 1) = y * y * (1.f - c) + c;
    		elementAt(1, 2) = y * z * (1.f - c) - (x * s);
    		elementAt(1, 3) = 0.f;

    		elementAt(2, 0) = x * z * (1.f - c) - (y * s);
    		elementAt(2, 1) = y * z * (1.f - c) + (x * s);
    		elementAt(2, 2) = z * z * (1.f - c) + c;
    		elementAt(2, 3) = 0.f;

    		elementAt(3, 0) = 0.f;
    		elementAt(3, 1) = 0.f;
    		elementAt(3, 2) = 0.f;
    		elementAt(3, 3) = 1.f;
    	}
    }

    void Matrix::rotate(const Vector &from, const Vector &to)
    {
    	// Creates a rotation matrix that will rotate the Vector 'from' into
    	// the Vector 'to'. The resulting matrix is stored in 'this' Matrix.
    	// For this method to work correctly, Vector 'from' and Vector 'to'
    	// must both be unit length Vectors.

    	assert(from.isUnitVector() && to.isUnitVector());
    	float e = from.dot(to);

    	if (CloseEnough(e, 1.f))
    	{
    		// Special case where 'from' is equal to 'to'. In other words,
    		// the angle between Vector 'from' and Vector 'to' is zero
    		// degrees. In this case the identity matrix is returned in
    		// Matrix 'result'.

    		loadIdentity();
    	}
    	else if (CloseEnough(e, -1.f))
    	{
    		// Special case where 'from' is directly opposite to 'to'. In
    		// other words, the angle between Vector 'from' and Vector 'to'
    		// is 180 degrees. In this case, the following matrix is used:
    		//
    		// Let:
    		//   F = from
    		//   S = vector perpendicular to F
    		//   U = S X F
    		//
    		// We want to rotate from (F, U, S) to (-F, U, -S)
    		//
    		// | -FxFx+UxUx-SxSx  -FxFy+UxUy-SxSy  -FxFz+UxUz-SxSz  0 |
    		// | -FxFy+UxUy-SxSy  -FyFy+UyUy-SySy  -FyFz+UyUz-SySz  0 |
    		// | -FxFz+UxUz-SxSz  -FyFz+UyUz-SySz  -FzFz+UzUz-SzSz  0 |
    		// |       0                 0                0         1 |

    		Vector side(0.f, from.z, -from.y);
    		if (CloseEnough(side.dot(side), 0.f))
    		{
    			side.x = -from.z;
    			side.y = 0.f;
    			side.z = from.x;
    		}

    		side.normalize();

    		Vector up;
    		up.cross(side, from);
    		up.normalize();

    		elementAt(0, 0) = -(from.x * from.x) + (up.x * up.x) - (side.x * side.x);
    		elementAt(0, 1) = -(from.x * from.y) + (up.x * up.y) - (side.x * side.y);
    		elementAt(0, 2) = -(from.x * from.z) + (up.x * up.z) - (side.x * side.z);
    		elementAt(0, 3) = 0.f;
    		elementAt(1, 0) = -(from.x * from.y) + (up.x * up.y) - (side.x * side.y);
    		elementAt(1, 1) = -(from.y * from.y) + (up.y * up.y) - (side.y * side.y);
    		elementAt(1, 2) = -(from.y * from.z) + (up.y * up.z) - (side.y * side.z);
    		elementAt(1, 3) = 0.f;
    		elementAt(2, 0) = -(from.x * from.z) + (up.x * up.z) - (side.x * side.z);
    		elementAt(2, 1) = -(from.y * from.z) + (up.y * up.z) - (side.y * side.z);
    		elementAt(2, 2) = -(from.z * from.z) + (up.z * up.z) - (side.z * side.z);
    		elementAt(2, 3) = 0.f;
    		elementAt(3, 0) = 0.f;
    		elementAt(3, 1) = 0.f;
    		elementAt(3, 2) = 0.f;
    		elementAt(3, 3) = 1.f;
    	}
    	else
    	{
    		// This is the most common case. Creates the rotation matrix:
    		//
    		//               | E + HVx^2    HVxVy - Vz   HVxVz + Vy   0 |
    		// R(from, to) = | HVxVy + Vz   E + HVy^2    HVyVz - Vx   0 |
    		//               | HVxVz - Vy   HVyVz + Vx   E + HVz^2    0 |
    		//               |     0            0            0        1 |
    		//
    		// where,
    		//   V = from X to
    		//   E = from.dot(to)
    		//   H = (1 - E) / V.dot(V)

    		Vector v;
    		v.cross(from, to);
    		v.normalize();

    		float h = (1.f - e) / v.dot(v);

    		elementAt(0, 0) = e + h * v.x * v.x;
    		elementAt(0, 1) = h * v.x * v.y - v.z;
    		elementAt(0, 2) = h * v.x * v.z + v.y;
    		elementAt(0, 3) = 0.f;

    		elementAt(1, 0) = h * v.x * v.y + v.z;
    		elementAt(1, 1) = e + h * v.y * v.y;
    		elementAt(1, 2) = h * v.y * v.z - v.x;
    		elementAt(1, 3) = 0.f;

    		elementAt(2, 0) = h * v.x * v.z - v.y;
    		elementAt(2, 1) = h * v.y * v.z + v.x;
    		elementAt(2, 2) = e + h * v.z * v.z;
    		elementAt(2, 3) = 0.f;

    		elementAt(3, 0) = 0.f;
    		elementAt(3, 1) = 0.f;
    		elementAt(3, 2) = 0.f;
    		elementAt(3, 3) = 1.f;
    	}
    }

    //------------------------------------------------------------------------
    // struct Point
    //------------------------------------------------------------------------

    Point::Point()
    {
    	x = y = z = 0.f;
    	w = 1.f;
    }

    Point::~Point() {}

    Point::Point(float x, float y, float z)
    {
    	set(x, y, z);
    }

    Point::Point(const Point &rhs)
    {
    	set(rhs.x, rhs.y, rhs.z);
    }

    void Point::set(float x, float y, float z)
    {
    	this->x = x;
    	this->y = y;
    	this->z = z;
    	w = 1.f;
    }

    void Point::multiply(const Point &p, const Matrix &m)
    {
    	// Applies the transformation in Matrix 'm' to Point 'p' and
    	// stores the transformed Point in 'this' Point.

    	float x = (m(0, 0) * p.x) + (m(0, 1) * p.y) + (m(0, 2) * p.z) + (m(0, 3) * p.w);
    	float y = (m(1, 0) * p.x) + (m(1, 1) * p.y) + (m(1, 2) * p.z) + (m(1, 3) * p.w);
    	float z = (m(2, 0) * p.x) + (m(2, 1) * p.y) + (m(2, 2) * p.z) + (m(2, 3) * p.w);
    	float w = (m(3, 0) * p.x) + (m(3, 1) * p.y) + (m(3, 2) * p.z) + (m(3, 3) * p.w);

    	this->x = x;
    	this->y = y;
    	this->z = z;
    	this->w = w;
    }

    Point &Point::operator=(const Point &rhs)
    {
    	if (this != &rhs)
    	{
    		x = rhs.x;
    		y = rhs.y;
    		z = rhs.z;
    		w = rhs.w;
    	}

    	return *this;
    }

    bool Point::operator==(const Point &rhs) const
    {
    	return CloseEnough(x, rhs.x) && CloseEnough(y, rhs.y)
    		&& CloseEnough(z, rhs.z) && CloseEnough(w, rhs.w);
    }

    //------------------------------------------------------------------------
    // struct Vector
    //------------------------------------------------------------------------

    Vector::Vector()
    {
    	x = y = z = w = 0.f;
    }

    Vector::~Vector() {}

    Vector::Vector(const Point &from, const Point &to)
    {
    	set(from, to);
    }

    Vector::Vector(float x, float y, float z)
    {
    	set(x, y, z);
    }

    Vector::Vector(const Vector &rhs)
    {
    	set(rhs.x, rhs.y, rhs.z);
    }

    void Vector::add(const Vector &v)
    {
    	// Adds vector 'v' to 'this' vector.

    	x += v.x;
    	y += v.y;
    	z += v.z;
    	w = 0.f;
    }

    void Vector::add(const Vector &v1, const Vector &v2)
    {
    	// Adds vector 'v1' to vector 'v2' and stores the resulting vector
    	// in vector 'this'. The vectors are added in the order:
    	// 'v1' + 'v2' = 'this'.

    	Vector tmp(v1);
    	tmp.add(v2);
    	*this = tmp;
    }

    float Vector::angle(const Vector &other) const
    {
    	// Finds the angle between vector 'this' and vector 'other'.
    	// The angle returned is in degrees.

    	float a = (x * other.x) + (y * other.y) + (z * other.z);
    	float b = length() * other.length();

    	return Radians2Degrees(acosf(a / b));
    }

    void Vector::set(float x, float y, float z)
    {
    	// Creates a vector from three cartesian coordinates.

    	this->x = x;
    	this->y = y;
    	this->z = z;
    	this->w = 0.f;
    }

    void Vector::set(const Point &from, const Point &to)
    {
    	// Creates a vector from point 'from' to point 'to'.

    	x = to.x - from.x;
    	y = to.y - from.y;
    	z = to.z - from.z;
    	w = 0.f;
    }

    void Vector::cross(const Vector &v)
    {
    	// Calculates the cross product: 'this' x 'v'.

    	Vector temp;

    	temp.x = (y * v.z) - (z * v.y);
    	temp.y = (z * v.x) - (x * v.z);
    	temp.z = (x * v.y) - (y * v.x);

    	*this = temp;
    }

    void Vector::cross(const Vector &v1, const Vector &v2)
    {
    	// Calculates the cross product: 'v1' x 'v2'.

    	Vector tmp(v1);
    	tmp.cross(v2);
    	*this = tmp;
    }

    void Vector::divide(const float scalar)
    {
    	// Divides each component of this vector by the scalar value.

    	assert(scalar != 0.f);
    	x /= scalar;
    	y /= scalar;
    	z /= scalar;
    	w /= scalar;
    }

    float Vector::dot(const Vector &other) const
    {
    	// Calculates the dot product: 'this' dot 'other'.
    	// The result can be used to determine if vector 'this' is
    	// orthogonal (or perpendicular) to vector 'other':
    	//   0 if angle between 'this' and 'other' equals 90 degrees.
    	// < 0 if angle between 'this' and 'other' less than 90 degrees.
    	// > 0 if angle between 'this' and 'other' greater than 90 degrees.

    	return (x * other.x) + (y * other.y) + (z * other.z);
    }

    float Vector::dot(const Point &other) const
    {
    	// The point 'other' is interpreted as a vector from the origin to
    	// the point 'other'.

    	return (x * other.x) + (y * other.y) + (z * other.z);
    }

    bool Vector::isUnitVector() const
    {
    	// Returns true if 'this' vector is of unit length.

    	return CloseEnough(length(), 1.f);
    }

    bool Vector::isZeroVector() const
    {
    	// Returns true if 'this' vector's (x,y,z,w) components are all zero.

    	return CloseEnough(x, 0.f) && CloseEnough(y, 0.f)
    		&& CloseEnough(z, 0.f) && CloseEnough(w, 0.f);
    }

    float Vector::length() const
    {
    	// Returns the length (magnitude) of the vector.

    	return sqrtf((x * x) + (y * y) + (z * z));
    }

    void Vector::multiply(const float scalar)
    {
    	// Multiplies each component of this vector by the scalar value.

    	x *= scalar;
    	y *= scalar;
    	z *= scalar;
    	w *= scalar;
    }

    void Vector::multiply(const Vector &v, const Matrix &m)
    {
    	// Applies the transformation in Matrix 'm' to Vector 'v' and
    	// stores the transformed Vector in 'this' Vector.

    	float x = (m(0, 0) * v.x) + (m(0, 1) * v.y) + (m(0, 2) * v.z) + (m(0, 3) * v.w);
    	float y = (m(1, 0) * v.x) + (m(1, 1) * v.y) + (m(1, 2) * v.z) + (m(1, 3) * v.w);
    	float z = (m(2, 0) * v.x) + (m(2, 1) * v.y) + (m(2, 2) * v.z) + (m(2, 3) * v.w);
    	float w = (m(3, 0) * v.x) + (m(3, 1) * v.y) + (m(3, 2) * v.z) + (m(3, 3) * v.w);

    	this->x = x;
    	this->y = y;
    	this->z = z;
    	this->w = w;
    }

    void Vector::normalize()
    {
    	// Converts the vector to unit length.

    	float length = sqrtf((x * x) + (y * y) + (z * z));

    	assert(length != 0.f);
    	length = 1.f / length;

    	x *= length;
    	y *= length;
    	z *= length;
    }

    void Vector::subtract(const Vector &v)
    {
    	// Subtracts vector 'this' from vector 'v'.

    	Vector temp;

    	temp.x = x - v.x;
    	temp.y = y - v.y;
    	temp.z = z - v.z;

    	*this = temp;
    }

    void Vector::subtract(const Vector &v1, const Vector &v2)
    {
    	// Subtracts vector 'v1' from vector 'v2' and stores the
    	// resulting vector in vector 'this'. The vectors are subtracted
    	// in the order: 'v1' - 'v2' = 'this'.

    	Vector tmp(v1);
    	tmp.subtract(v2);
    	*this = tmp;
    }

    Vector &Vector::operator=(const Vector &rhs)
    {
    	if (this != &rhs)
    	{
    		x = rhs.x;
    		y = rhs.y;
    		z = rhs.z;
    		w = rhs.w;
    	}

    	return *this;
    }

    bool Vector::operator==(const Vector &rhs) const
    {
    	return CloseEnough(x, rhs.x) && CloseEnough(y, rhs.y)
    		&& CloseEnough(z, rhs.z) && CloseEnough(w, rhs.w);
    }

    //------------------------------------------------------------------------
    // struct Quaternion
    //------------------------------------------------------------------------
    // References:
    // 1. Bobick, Nick. "Rotating Objects Using Quaternions," Game Developer,
    //    February 1998.
    //
    // 2. Shoemake, Ken. "Animating Rotation with Quaternion Curves,"
    //    Computer Graphics, 19 (3), July 1985. (SIGGRAPH '85 Proceedings),
    //    245-254.
    //
    // 3. Shoemake, Ken. "Quaternion Calculus For Animation," Notes for
    //    Course #23, Math for SIGGRAPH, SIGGRAPH '89.
    //------------------------------------------------------------------------

    Quaternion::Quaternion()
    {
    	x = y = z = w = 0.f;
    }

    Quaternion::~Quaternion() {}

    Quaternion::Quaternion(const Quaternion &rhs)
    {
    	x = rhs.x;
    	y = rhs.y;
    	z = rhs.z;
    	w = rhs.w;
    }

    Quaternion::Quaternion(float x, float y, float z, float w)
    {
    	this->x = x;
    	this->y = y;
    	this->z = z;
    	this->w = w;
    }

    Quaternion::Quaternion(const Vector &axis, float angle)
    {
    	fromAxisAngle(axis, angle);
    }

    Quaternion::Quaternion(float rx, float ry, float rz)
    {
    	fromEuler(rx, ry, rz);
    }

    Quaternion::Quaternion(const Matrix &m)
    {
    	fromRotationMatrix(m);
    }

    void Quaternion::add(const Quaternion &q)
    {
    	// Adds quaternion 'q' to 'this' quaternion.

    	x += q.x;
    	y += q.y;
    	z += q.z;
    	w += q.w;

    	normalize();
    }

    void Quaternion::add(const Quaternion &q1, const Quaternion &q2)
    {
    	// Adds quaternion 'q1' to quaternion 'q2' and stores the resulting
    	// quaternion in 'this'. The two quaternions to add must both already
    	// be unit quaternions. The quaternions are added in the order:
    	// this + other = result

    	Quaternion tmp(q1);
    	tmp.add(q2);
    	*this = tmp;
    }

    void Quaternion::conjugate(const Quaternion &q)
    {
    	// Finds the conjugate of quaternion 'q'.

    	x = -q.x;
    	y = -q.y;
    	z = -q.z;
    	w = q.w;
    }

    void Quaternion::fromAxisAngle(const Vector &axis, float angle)
    {
    	// Creates a quaternion from an axis (x, y, z) and an angle.
    	// The angle must be in degrees.

    	// Normalize the axis of rotation.
    	Vector unitAxis = axis;
    	unitAxis.normalize();

    	// Assemble the quaternion.
    	angle = Degrees2Radians(angle);
    	float sine = sinf(angle);
    	float cosine = cosf(angle);

    	x = unitAxis.x * sine;
    	y = unitAxis.y * sine;
    	z = unitAxis.z * sine;
    	w = unitAxis.z * cosine;

    	normalize();
    }

    void Quaternion::fromEuler(float rx, float ry, float rz)
    {
    	// Creates a quaternion from an Euler transform. 'rx' is the
    	// rotation about the x-axis (also called pitch). 'ry' is the
    	// rotation about the y-axis (also called heading or yaw).
    	// 'rz' is the rotation about the z-axis (also called roll).
    	// The rotation about each axis must be in degrees.

    	rx = Degrees2Radians(rx) * 0.5f;
    	ry = Degrees2Radians(ry) * 0.5f;
    	rz = Degrees2Radians(rz) * 0.5f;

    	float cx = cosf(rx);
    	float cy = cosf(ry);
    	float cz = cosf(rz);
    	float sx = sinf(rx);
    	float sy = sinf(ry);
    	float sz = sinf(rz);

    	float cc = cx * cz;
    	float cs = cx * sz;
    	float sc = sx * cz;
    	float ss = sx * sz;

    	x = cy * sc - sy * cs;
    	y = cy * ss + sy * cc;
    	z = cy * cs - sy * sc;
    	w = cy * cc + sy * ss;

    	normalize();
    }

    void Quaternion::fromRotationMatrix(const Matrix &m)
    {
    	// Creates a quaternion from a rotation matrix.

    	float trace = m(0,0) + m(1,1) + m(2,2);

    	if (trace > 0.f)
    	{
    		float s = sqrtf(trace + 1.f);

    		w = s * 0.5f;

    		assert(s != 0.f);
    		s = 0.5f / s;

    		x = (m(1,2) - m(2,1)) * s;
    		y = (m(2,0) - m(0,2)) * s;
    		z = (m(0,1) - m(1,0)) * s;
    	}
    	else
    	{
    		int nxt[] = {1, 2, 0};
    		int i = 0;

    		if (m(1,1) > m(0,0))
    			i = 1;

    		if (m(2,2) > m(i,i))
    			i = 2;

    		int j = nxt[i];
    		int k = nxt[j];
    		float s = sqrtf((m(i,i) - (m(j,j) + m(k,k)) + 1.f));
    		float q[4];

    		q[i] = s * 0.5f;

    		assert(s != 0.f);
    		s = 0.5f / s;

    		q[3] = (m(j,k) - m(k,j)) * s;
    		q[j] = (m(i,j) + m(j,i)) * s;
    		q[k] = (m(i,k) + m(k,i)) * s;

    		x = q[0];
    		y = q[1];
    		z = q[2];
    		w = q[3];
    	}
    }

    void Quaternion::inverse(const Quaternion &q)
    {
    	// Find the inverse of quaternion 'q'.

    	float lgth = q.length();

    	assert(lgth != 0.f);
    	lgth = 1.f / lgth;

    	x = -q.x * lgth;
    	y = -q.y * lgth;
    	z = -q.z * lgth;
    	w = q.w * lgth;
    }

    bool Quaternion::isUnitQuaternion() const
    {
    	// Checks if the quaternion is of unit length.

    	return CloseEnough(length(), 1.f);
    }

    float Quaternion::length() const
    {
    	// Returns the length (magnitude) of the quaternion.

    	return sqrtf((x * x) + (y * y) + (z * z) + (w * w));
    }

    void Quaternion::multiply(Quaternion &q)
    {
    	// Multiplies 'this' quaternion with the quaternion 'q' and stores
    	// the result in quaternion 'this'. It is assumed that the two
    	// quaternions to be multiplied are unit quaternions. The
    	// quaternions are multiplied in the order: this = this * q

    	Quaternion temp;

    	temp.x = (w * q.x) + (x * q.w) + (y * q.z) - (z * q.y);
    	temp.y = (w * q.y) + (y * q.w) + (z * q.x) - (x * q.z);
    	temp.z = (w * q.z) + (z * q.w) + (x * q.y) - (y * q.x);
    	temp.w = (w * q.w) - (x * q.x) - (y * q.y) - (z * q.z);

    	temp.normalize();
    	*this = temp;
    }

    void Quaternion::multiply(Quaternion &q1, Quaternion &q2)
    {
    	// Multiplies quaternion 'q1' with the quaternion 'q2' and stores
    	// the result in quaternion 'this'. It is assumed that the two
    	// quaternions to be multiplied are unit quaternions. The
    	// quaternions are multiplied in the order: this = q1 * q2

    	Quaternion tmp(q1);
    	tmp.multiply(q2);
    	*this = tmp;
    }

    void Quaternion::normalize()
    {
    	// Converts the quaternion to unit length.

    	float lgth = sqrtf((x * x) + (y * y) + (z * z) + (w * w));

    	assert(lgth != 0.f);
    	lgth = 1.f / lgth;

    	x *= lgth;
    	y *= lgth;
    	z *= lgth;
    	w *= lgth;
    }

    void Quaternion::set(float x, float y, float z, float w)
    {
    	this->x = x;
    	this->y = y;
    	this->z = z;
    	this->w = w;
    }

    void Quaternion::slerp(const Quaternion &from, const Quaternion &to, float t)
    {
    	// Smoothly interpolates from quaternion 'from' to quaternion 'to'
    	// using spherical linear interpolation (slerp). The resulting
    	// slerp quaternion is stored in 'this' quaternion.
    	// As 't' goes from 0 to 1, the slerp quaternion goes from 'from'
    	// to 'to'.

    	float cosom = (from.x * to.x) + (from.y * to.y)
    		+ (from.z * to.z) + (from.w * to.w);

    	Quaternion temp = to;

    	// Adjust sign (if necessary).
    	if (cosom < 0.f)
    	{
    		cosom = -cosom;
    		temp.x = -temp.x;
    		temp.y = -temp.y;
    		temp.z = -temp.z;
    		temp.w = -temp.w;
    	}

    	float scale0;
    	float scale1;

    	// Calculate coefficients.
    	if (CloseEnough(1.f - cosom, 0.f))
    	{
    		// 'from' and 'to' very close...linearly interpolate.
    		scale0 = 1.f - t;
    		scale1 = t;
    	}
    	else
    	{
    		// Standard case...slerp.
    		float omega = acosf(cosom);
    		float sinom = sinf(omega);

    		scale0 = sinf((1.f - t) * omega) / sinom;
    		scale1 = sinf(t * omega) / sinom;
    	}

    	// Calculate final slerp quaternion.
    	x = (scale0 * from.x) + (scale1 * temp.x);
    	y = (scale0 * from.y) + (scale1 * temp.y);
    	z = (scale0 * from.z) + (scale1 * temp.z);
    	w = (scale0 * from.w) + (scale1 * temp.w);
    }

    void Quaternion::toAxisAngle(Vector &axis, float &angle) const
    {
    	// Returns the axis-angle representation for 'this' quaternion.
    	// The axis of rotation is returned in vector 'axis'. The angle
    	// of rotation is returned in 'angle' and will be in degrees.

    	float lgth = length();

    	if (CloseEnough(lgth, 0.f))
    	{
    		axis.x = 0.f;
    		axis.y = 0.f;
    		axis.z = 1.f;
    		axis.w = 0.f;
    		angle = 0.f;
    	}
    	else
    	{
    		lgth = 1.f / lgth;
    		axis.x = x * lgth;
    		axis.y = y * lgth;
    		axis.z = z * lgth;
    		axis.w = 0.f;
    		angle = Radians2Degrees(2.f * acosf(w));
    	}
    }

    void Quaternion::toRotationMatrix(Matrix &m) const
    {
    	// Returns the rotation matrix for this quaternion.
    	// The rotation matrix is returned in matrix 'm'.
    	//
    	//  | 1 - 2(y^2 + z^2)  2(xy - wz)         2(xy + wy)        0 |
    	//  | 2(xy + wz)        1 - 2(x^2 + z^2)   2(yz - wx)        0 |
    	//  | 2(xz - wy)        2(yz + wx)         1 - 2(x^2 + y^2)  0 |
    	//  |      0                  0                  0           1 |
    	//

    	float x2 = x + x;
    	float y2 = y + y;
    	float z2 = z + z;
    	float xx = x * x2;
    	float xy = x * y2;
    	float xz = x * z2;
    	float yy = y * y2;
    	float yz = y * z2;
    	float zz = z * z2;
    	float wx = w * x2;
    	float wy = w * y2;
    	float wz = w * z2;

    	m(0,0) = 1.f - (yy + zz);
    	m(0,1) = xy - wz;
    	m(0,2) = xz + wy;
    	m(0,3) = 0.f;

    	m(1,0) = xy + wz;
    	m(1,1) = 1.f - (xx + zz);
    	m(1,2) = yz - wx;
    	m(1,3) = 0.f;

    	m(2,0) = xz - wy;
    	m(2,1) = yz + wx;
    	m(2,2) = 1.f - (xx + yy);
    	m(2,3) = 0.f;

    	m(3,0) = 0.f;
    	m(3,1) = 0.f;
    	m(3,2) = 0.f;
    	m(3,3) = 1.f;
    }

    Quaternion &Quaternion::operator=(const Quaternion &rhs)
    {
    	if (this != &rhs)
    	{
    		x = rhs.x;
    		y = rhs.y;
    		z = rhs.z;
    		w = rhs.w;
    	}

    	return *this;
    }

    bool Quaternion::operator==(const Quaternion &rhs) const
    {
    	return CloseEnough(x, rhs.x) && CloseEnough(y, rhs.y)
    		&& CloseEnough(z, rhs.z) && CloseEnough(w, rhs.w);
    }

    //------------------------------------------------------------------------
    // struct Plane
    //------------------------------------------------------------------------

    Plane::Plane()
    {
    	distOrigin = 0.f;
    }

    Plane::~Plane() {}

    Plane::Plane(const Plane &rhs)
    {
    	normal = rhs.normal;
    	distOrigin = rhs.distOrigin;
    }

    Plane::Plane(const Point &p1, const Point &p2, const Point &p3)
    {
    	set(p1, p2, p3);
    }

    void Plane::set(const Point &p1, const Point &p2, const Point &p3)
    {
    	// Find the normal to the plane.
    	Vector u, v;
    	u.set(p1, p2);
    	v.set(p1, p3);
    	normal.cross(u, v);
    	normal.normalize();

    	// Find the distance of the plane to the origin.
    	distOrigin = -normal.dot(p1);
    }

    float Plane::distanceTo(const Point &p)
    {
    	// Calculates the smallest distance from the point 'p' to this
    	// plane. The distance returned can be interpreted as:
    	//   0 if the point 'p' is located in this plane
    	// > 0 if the point 'p' is in front of this plane
    	// < 0 if the point 'p' is behind this plane

    	return normal.dot(p) + distOrigin;
    }

    Plane &Plane::operator=(const Plane &rhs)
    {
    	if (this != &rhs)
    	{
    		normal = rhs.normal;
    		distOrigin = rhs.distOrigin;
    	}

    	return *this;
    }

    bool Plane::operator==(const Plane &rhs) const
    {
    	return (normal == rhs.normal)
    		&& CloseEnough(distOrigin, rhs.distOrigin);
    }

Currently browsing [mathlib.zip] (51,635 bytes) - [mathlib/mathlib.hpp] - (6,475 bytes)

    //------------------------------------------------------------------------
    // Math Library for right-handed 3D graphics APIs.
    // Copyright (C) 2001-2002 David Poon
    //
    // This library is free software; you can redistribute it and/or
    // modify it under the terms of the GNU Lesser General Public
    // License as published by the Free Software Foundation; either
    // version 2.1 of the License, or (at your option) any later version.
    //
    // This library is distributed in the hope that it will be useful,
    // but WITHOUT ANY WARRANTY; without even the implied warranty of
    // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    // GNU Lesser General Public License for more details.
    //
    // You should have received a copy of the GNU Lesser General
    // Public License along with this library; if not, write to the
    // Free Software Foundation, Inc., 59 Temple Place, Suite 330,
    // Boston, MA 02111-1307 USA
    //------------------------------------------------------------------------

    #ifndef MATH_LIBRARY_RIGHT_HANDED_HPP
    #define MATH_LIBRARY_RIGHT_HANDED_HPP

    #include <cmath>
    #include <limits>

    extern const float PI;

    //------------------------------------------------------------------------
    // Math library inlined functions
    //------------------------------------------------------------------------

    inline bool CloseEnough(const float &f1, const float &f2)
    {
    	// Determines whether the two floating point values f1 and f2 are
    	// close enough together that they can be considered equal.
    	// This function is taken from Pete Becker's "The Journeyman's Shop"
    	// column published in C/C++ Users Journal, December 2000. It is
    	// presented here slightly modified to deal exclusively with floats.
    	return fabs((f1 - f2) / ((f2 == 0.f) ? 1.f : f2))
    		< std::numeric_limits<float>::epsilon();
    }

    inline float Degrees2Radians(float angle)
    {
    	// Converts the angle (in degrees) to radians.
    	return (angle * PI) / 180.f;
    }

    inline float Radians2Degrees(float angle)
    {
    	// Converts the angle (in radians) to degrees.
    	return (angle * 180.f) / PI;
    }

    //------------------------------------------------------------------------
    // Definitions of common 3D data structures for use in 3D graphics.
    // These data structures are suitable for graphics APIs that use a
    // right-handed cartesian coordinate system (i.e., OpenGL).
    //------------------------------------------------------------------------

    class Matrix;
    struct Point;
    struct Vector;
    struct Quaternion;
    struct Plane;

    class Matrix
    {
    public:
    	Matrix();
    	Matrix(const Matrix &rhs);
    	~Matrix();

    	float &operator()(int row, int col) {return elementAt(row, col);}
    	const float &operator()(int row, int col) const {return elementAt(row, col);}

    	void add(const Matrix &m);
    	void add(const Matrix &m1, const Matrix &m2);
    	void subtract(const Matrix &m);
    	void subtract(const Matrix &m1, const Matrix &m2);
    	void multiply(float scalar);
    	void multiply(const Matrix &m);
    	void multiply(const Matrix &m1, const Matrix &m2);

    	Matrix &operator=(const Matrix &rhs);
    	bool operator==(const Matrix &rhs) const;
    	bool operator!=(const Matrix &rhs) const;

    	operator float*() {return m_mtx;}
    	operator const float*() const {return m_mtx;}

    	bool inverse(const Matrix &m);
    	void loadIdentity();
    	void transpose(const Matrix &m);

    	void rotate(float angle, Vector &axis);
    	void rotate(const Vector &from, const Vector &to);
    	void scale(float sx, float sy, float sz);
    	void translate(float tx, float ty, float tz);

    private:
    	float &elementAt(int row, int col) {return m_mtx[col*4+row];}
    	const float &elementAt(int row, int col) const {return m_mtx[col*4+row];}

    	float m_mtx[16];
    };

    struct Point
    {
    	float x, y, z, w;

    	Point();
    	Point(const Point &rhs);
    	Point(float x, float y, float z);
    	~Point();

    	void set(float x, float y, float z);

    	void multiply(const Point &p, const Matrix &m);

    	Point &operator=(const Point &rhs);
    	bool operator==(const Point &rhs) const;
    };

    struct Vector
    {
    	float x, y, z, w;

    	Vector();
    	Vector(const Vector &rhs);
    	Vector(float x, float y, float z);
    	Vector(const Point &from, const Point &to);
    	~Vector();

    	void set(float x, float y, float z);
    	void set(const Point &from, const Point &to);

    	void add(const Vector &v);
    	void add(const Vector &v1, const Vector &v2);
    	void cross(const Vector &v);
    	void cross(const Vector &v1, const Vector &v2);
    	void divide(const float scalar);
    	float dot(const Vector &other) const;
    	float dot(const Point &other) const;
    	void multiply(const float scalar);
    	void multiply(const Vector &v, const Matrix &m);
    	void subtract(const Vector &v);
    	void subtract(const Vector &v1, const Vector &v2);

    	float angle(const Vector &other) const;
    	bool isUnitVector() const;
    	bool isZeroVector() const;
    	float length() const;
    	void normalize();

    	Vector &operator=(const Vector &rhs);
    	bool operator==(const Vector &rhs) const;
    };

    struct Quaternion
    {
    	float x, y, z, w;

    	Quaternion();
    	Quaternion(const Quaternion &rhs);
    	Quaternion(float x, float y, float z, float w);
    	Quaternion(const Vector &axis, float angle);
    	Quaternion(float rx, float ry, float rz);
    	Quaternion(const Matrix &m);
    	~Quaternion();

    	void add(const Quaternion &q);
    	void add(const Quaternion &q1, const Quaternion &q2);
    	void multiply(Quaternion &q);
    	void multiply(Quaternion &q1, Quaternion &q2);
    	void inverse(const Quaternion &q);
    	void conjugate(const Quaternion &q);

    	void fromAxisAngle(const Vector &axis, float angle);
    	void fromEuler(float rx, float ry, float rz);
    	void fromRotationMatrix(const Matrix &m);
    	bool isUnitQuaternion() const;
    	float length() const;
    	void normalize();
    	void set(float x, float y, float z, float w);
    	void slerp(const Quaternion &from, const Quaternion &to, float t);
    	void toAxisAngle(Vector &axis, float &angle) const;
    	void toRotationMatrix(Matrix &m) const;

    	Quaternion &operator=(const Quaternion &rhs);
    	bool operator==(const Quaternion &rhs) const;
    };

    struct Plane
    {
    	Vector normal;
    	float distOrigin;	// smallest distance from plane to origin

    	Plane();
    	Plane(const Plane &rhs);
    	Plane(const Point &p1, const Point &p2, const Point &p3);
    	~Plane();

    	void set(const Point &p1, const Point &p2, const Point &p3);
    	float distanceTo(const Point &p);

    	Plane &operator=(const Plane &rhs);
    	bool operator==(const Plane &rhs) const;
    };

    #endif

Currently browsing [mathlib.zip] (51,635 bytes) - [mathlib/test.cpp] - (11,444 bytes)

    //------------------------------------------------------------------------
    // Tester for the 3D Math Library.
    // Copyright (C) 2001-2002 David Poon
    //
    // This library is free software; you can redistribute it and/or
    // modify it under the terms of the GNU Lesser General Public
    // License as published by the Free Software Foundation; either
    // version 2.1 of the License, or (at your option) any later version.
    //
    // This library is distributed in the hope that it will be useful,
    // but WITHOUT ANY WARRANTY; without even the implied warranty of
    // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    // GNU Lesser General Public License for more details.
    //
    // You should have received a copy of the GNU Lesser General
    // Public License along with this library; if not, write to the
    // Free Software Foundation, Inc., 59 Temple Place, Suite 330,
    // Boston, MA 02111-1307 USA
    //------------------------------------------------------------------------

    #include <cstdio>
    #include "mathlib.hpp"

    bool TestCastToFloatPtr(float *array = 0)
    {
    	if (!array)
    		return false;

    	return true;
    }

    void TestMatrix()
    {
    	/*
    	Expected Output for TestMatrix():
    	----------------------------------------------
    	Testing Matrix portion of Maths Library...
    	Test #1: Matrix float* cast operator PASSED
    	Test #2: Matrix (row, col) operator PASSED
    	Test #3: Matrix multiplication operator PASSED
    	Test #4: Matrix transpose PASSED
    	Test #5: Matrix inverse PASSED
    	Test #6: Matrix addition PASSED
    	Test #7: Matrix subtraction PASSED
    	Test #8: Matrix multiplication by scalar PASSED
    	Test #9: (1.f / 10.f) * 10.f == 1.f  PASSED
    	Test #10: Build matrix to rotate from one vector to another
    			Rotate(from, to) Case 1 PASSED
    			Rotate(from, to) Case 2 PASSED
    			Rotate(from, to) Case 3 PASSED
    	Test #11: Build translation matrix PASSED
    	Test #12: Build scaling matrix PASSED
    	Test #13: Build rotation matrix PASSED
    	*/

    	puts("\nTesting Matrix portion of Maths Library...");

    	// Test #1: Matrix float* cast operator.
    	{
    		Matrix m;
    		if (!TestCastToFloatPtr(m))
    			puts("Test #1: Matrix float* cast operator FAILED");
    		else
    			puts("Test #1: Matrix float* cast operator PASSED");
    	}

    	// Test #2: Matrix (row, col) operator.
    	{
    		Matrix m;
    		m(1, 2) = 12.f;
    		if (!CloseEnough(m(1, 2), 12.f))
    			puts("Test #2: Matrix (row, col) operator FAILED");
    		else
    			puts("Test #2: Matrix (row, col) operator PASSED");
    	}

    	// Test #3: Matrix multiplication operator.
    	{
    		Matrix m;
    		m(0,0) = 1.f;	m(0,1) = 1.f;	m(0,2) = 1.f;	m(0,3) = 1.f;
    		m(1,0) = 2.f;	m(1,1) = 2.f;	m(1,2) = 2.f;	m(1,3) = 2.f;
    		m(2,0) = 3.f;	m(2,1) = 3.f;	m(2,2) = 3.f;	m(2,3) = 3.f;
    		m(3,0) = 4.f;	m(3,1) = 4.f;	m(3,2) = 4.f;	m(3,3) = 4.f;

    		Matrix identity, result;

    		identity.loadIdentity();
    		result.multiply(m, identity);

    		if (result != m)
    			puts("Test #3: Matrix multiplication operator FAILED");
    		else
    			puts("Test #3: Matrix multiplication operator PASSED");
    	}

    	// Test #4: Matrix transpose.
    	{
    		Matrix m; // original matrix
    		m(0,0) = 1.f;	m(0,1) = 1.f;	m(0,2) = 1.f;	m(0,3) = 1.f;
    		m(1,0) = 2.f;	m(1,1) = 2.f;	m(1,2) = 2.f;	m(1,3) = 2.f;
    		m(2,0) = 3.f;	m(2,1) = 3.f;	m(2,2) = 3.f;	m(2,3) = 3.f;
    		m(3,0) = 4.f;	m(3,1) = 4.f;	m(3,2) = 4.f;	m(3,3) = 4.f;

    		Matrix t; // the expected transpose of m
    		t(0,0) = 1.f;	t(0,1) = 2.f;	t(0,2) = 3.f;	t(0,3) = 4.f;
    		t(1,0) = 1.f;	t(1,1) = 2.f;	t(1,2) = 3.f;	t(1,3) = 4.f;
    		t(2,0) = 1.f;	t(2,1) = 2.f;	t(2,2) = 3.f;	t(2,3) = 4.f;
    		t(3,0) = 1.f;	t(3,1) = 2.f;	t(3,2) = 3.f;	t(3,3) = 4.f;

    		Matrix result;
    		result.transpose(m);

    		if (result != t)
    			puts("Test #4: Matrix transpose FAILED");
    		else
    			puts("Test #4: Matrix transpose PASSED");
    	}

    	// Test #5: Matrix inverse.
    	{
    		Matrix identity; // identity matrix
    		identity.loadIdentity();

    		Matrix m; // original matrix.
    		m(0,0) = 1.f;	m(0,1) = -3.f;	m(0,2) = 0.f;	m(0,3) = -2.f;
    		m(1,0) = 3.f;	m(1,1) = -12.f;	m(1,2) = -2.f;	m(1,3) = -6.f;
    		m(2,0) = -2.f;	m(2,1) = 10.f;	m(2,2) = 2.f;	m(2,3) = 5.f;
    		m(3,0) = -1.f;	m(3,1) = 6.f;	m(3,2) = 1.f;	m(3,3) = 3.f;

    		Matrix i; // the expected inverse of m
    		i(0,0) = 0.f;	i(0,1) = 1.f;	i(0,2) = 0.f;	i(0,3) = 2.f;
    		i(1,0) = 1.f;	i(1,1) = -1.f;	i(1,2) = -2.f;	i(1,3) = 2.f;
    		i(2,0) = 0.f;	i(2,1) = 1.f;	i(2,2) = 3.f;	i(2,3) = -3.f;
    		i(3,0) = -2.f;	i(3,1) = 2.f;	i(3,2) = 3.f;	i(3,3) = -2.f;

    		Matrix inverse;
    		inverse.inverse(m);

    		if (inverse == i)
    		{
    			inverse.multiply(m);

    			if (inverse == identity)
    				puts("Test #5: Matrix inverse PASSED");
    			else
    				puts("Test #5: Matrix inverse FAILED");
    		}
    		else
    		{
    			puts("Test #5: Matrix inverse FAILED");
    		}
    	}

    	// Test #6: Matrix addition.
    	{
    		Matrix m1, m2;
    		m1.loadIdentity();
    		m2.loadIdentity();

    		Matrix m3; // expected result
    		m3(0,0) = 2.f;	m3(0,1) = 0.f;	m3(0,2) = 0.f;	m3(0,3) = 0.f;
    		m3(1,0) = 0.f;	m3(1,1) = 2.f;	m3(1,2) = 0.f;	m3(1,3) = 0.f;
    		m3(2,0) = 0.f;	m3(2,1) = 0.f;	m3(2,2) = 2.f;	m3(2,3) = 0.f;
    		m3(3,0) = 0.f;	m3(3,1) = 0.f;	m3(3,2) = 0.f;	m3(3,3) = 2.f;

    		m1.add(m2);

    		if (m1 != m3)
    			puts("Test #6: Matrix addition FAILED");
    		else
    			puts("Test #6: Matrix addition PASSED");
    	}

    	// Test #7: Matrix subtraction.
    	{
    		Matrix m1, m2;
    		m1.loadIdentity();
    		m2.loadIdentity();

    		Matrix m3; // expected result
    		m3(0,0) = 0.f;	m3(0,1) = 0.f;	m3(0,2) = 0.f;	m3(0,3) = 0.f;
    		m3(1,0) = 0.f;	m3(1,1) = 0.f;	m3(1,2) = 0.f;	m3(1,3) = 0.f;
    		m3(2,0) = 0.f;	m3(2,1) = 0.f;	m3(2,2) = 0.f;	m3(2,3) = 0.f;
    		m3(3,0) = 0.f;	m3(3,1) = 0.f;	m3(3,2) = 0.f;	m3(3,3) = 0.f;

    		m1.subtract(m2);

    		if (m1 != m3)
    			puts("Test #7: Matrix subtraction FAILED");
    		else
    			puts("Test #7: Matrix subtraction PASSED");
    	}

    	// Test #8: Matrix multiplication by scalar.
    	{
    		Matrix m1; // original matrix
    		m1(0,0) = 1.f;	m1(0,1) = 1.f;	m1(0,2) = 1.f;	m1(0,3) = 1.f;
    		m1(1,0) = 1.f;	m1(1,1) = 1.f;	m1(1,2) = 1.f;	m1(1,3) = 1.f;
    		m1(2,0) = 1.f;	m1(2,1) = 1.f;	m1(2,2) = 1.f;	m1(2,3) = 1.f;
    		m1(3,0) = 1.f;	m1(3,1) = 1.f;	m1(3,2) = 1.f;	m1(3,3) = 1.f;

    		Matrix m2; // expected result
    		m2(0,0) = 3.f;	m2(0,1) = 3.f;	m2(0,2) = 3.f;	m2(0,3) = 3.f;
    		m2(1,0) = 3.f;	m2(1,1) = 3.f;	m2(1,2) = 3.f;	m2(1,3) = 3.f;
    		m2(2,0) = 3.f;	m2(2,1) = 3.f;	m2(2,2) = 3.f;	m2(2,3) = 3.f;
    		m2(3,0) = 3.f;	m2(3,1) = 3.f;	m2(3,2) = 3.f;	m2(3,3) = 3.f;

    		m1.multiply(3.f);

    		if (m1 == m2)
    			puts("Test #8: Matrix multiplication by scalar PASSED");
    		else
    			puts("Test #8: Matrix multiplication by scalar FAILED");
    	}

    	// Test #9: Comparing floating points for equality.
    	{
    		float tenth = 1.f / 10.f;

    		if (tenth * 10.f != 1.f)
    		{
    			if (CloseEnough(tenth * 10.f, 1.f))
    				puts("Test #9: (1.f / 10.f) * 10.f == 1.f  PASSED");
    		}
    		else
    		{
    			puts("Test #9: (1.f / 10.f) * 10.f == 1.f  FAILED");
    		}
    	}

    	// Test #10: Rotation of vector FROM into vector TO.
    	{
    		puts("Test #10: Build matrix to rotate from one vector to another");

    		Vector temp;
    		Vector from;
    		Vector to;
    		Matrix m;

    		// Case 1: from = (1,0,0), to = (1,0,0)
    		from.set(1.f, 0.f, 0.f);
    		to = from;

    		m.rotate(from, to);
    		temp.multiply(from, m);

    		if (temp == to)
    			puts("\tRotate(from, to) Case 1 PASSED");
    		else
    			puts("\tRotate(from, to) Case 1 FAILED");

    		// Case 2: from = (1,0,0), to = (-1,0,0)
    		from.set(1.f, 0.f, 0.f);
    		to.set(-1.f, 0.f, 0.f);

    		m.rotate(from, to);
    		temp.multiply(from, m);

    		if (temp == to)
    			puts("\tRotate(from, to) Case 2 PASSED");
    		else
    			puts("\tRotate(from, to) Case 2 FAILED");

    		// Case 3: from = (1,0,0), to = (0,1,0)
    		from.set(1.f, 0.f, 0.f);
    		to.set(0.f, 1.f, 0.f);

    		m.rotate(from, to);
    		temp.multiply(from, m);

    		if (temp == to)
    			puts("\tRotate(from, to) Case 3 PASSED");
    		else
    			puts("\tRotate(from, to) Case 3 FAILED");
    	}

    	// Test #11: Build translation matrix.
    	{
    		Point p(0.f, 0.f, 0.f);
    		Point expected(7.f, 7.f, 7.f);

    		Matrix m;
    		m.translate(7.f, 7.f, 7.f);

    		Point result;
    		result.multiply(p, m);

    		if (result == expected)
    			puts("Test #11: Build translation matrix PASSED");
    		else
    			puts("Test #11: Build translation matrix FAILED");
    	}

    	// Test #12: Build scaling matrix.
    	{
    		Point p(1.f, 1.f, 1.f);
    		Point expected(7.f, 7.f, 7.f);

    		Matrix m;
    		m.scale(7.f, 7.f, 7.f);

    		Point result;
    		result.multiply(p, m);

    		if (result == expected)
    			puts("Test #12: Build scaling matrix PASSED");
    		else
    			puts("Test #12: Build scaling matrix FAILED");
    	}

    	// Test #13: Build rotation matrix.
    	{
    		Point p(1.f, 0.f, 0.f);
    		Point expected(0.f, 1.f, 0.f);

    		Matrix m;
    		Vector zAxis(0.f, 0.f, 1.f); // rotate about z-axis
    		m.rotate(90.f, zAxis);

    		Point result;
    		result.multiply(p, m);

    		if (result == expected)
    			puts("Test #13: Build rotation matrix PASSED");
    		else
    			puts("Test #13: Build rotation matrix FAILED");
    	}
    }

    void TestPoint()
    {
    	/*
    	Expected Output for TestPoint():
    	----------------------------------------------
    	Testing Point portion of Maths Library...
    	Test #1: Translate Point PASSED
    	Test #2: Scale Point PASSED
    	Test #3: Rotate Point PASSED
    	*/

    	puts("\nTesting Point portion of Maths Library...");

    	// Test #1: Translate Point.
    	{
    		Point p(0.f, 0.f, 0.f);
    		Point expected(7.f, 7.f, 7.f);

    		Matrix m;
    		m.translate(7.f, 7.f, 7.f);

    		Point result;
    		result.multiply(p, m);

    		if (result == expected)
    			puts("Test #1: Translate Point PASSED");
    		else
    			puts("Test #1: Translate Point FAILED");
    	}

    	// Test #2: Scale Point.
    	{
    		Point p(1.f, 1.f, 1.f);
    		Point expected(7.f, 7.f, 7.f);

    		Matrix m;
    		m.scale(7.f, 7.f, 7.f);

    		Point result;
    		result.multiply(p, m);

    		if (result == expected)
    			puts("Test #2: Scale Point PASSED");
    		else
    			puts("Test #2: Scale Point FAILED");
    	}

    	// Test #3: Rotate Point.
    	{
    		Point p(1.f, 0.f, 0.f);
    		Point expected(0.f, 1.f, 0.f);

    		Matrix m;
    		Vector zAxis(0.f, 0.f, 1.f); // rotate about z-axis
    		m.rotate(90.f, zAxis);

    		Point result;
    		result.multiply(p, m);

    		if (result == expected)
    			puts("Test #3: Rotate Point PASSED");
    		else
    			puts("Test #3: Rotate Point FAILED");
    	}
    }

    void TestVector()
    {
    	/*
    	Expected Output for TestVector():
    	----------------------------------------------
    	*/

    	puts("\nTesting Vector portion of Maths Library...");
    }

    void TestQuaternion()
    {
    	/*
    	Expected Output for TestQuaternion():
    	----------------------------------------------
    	*/

    	puts("\nTesting Quaternion portion of Maths Library...");
    }

    void TestPlane()
    {
    	/*
    	Expected Output for TestPlane():
    	----------------------------------------------
    	Planes PASSED
    	*/

    	puts("\nTesting Plane portion of Maths Library...");

    	Point points[] =
    	{
    		Point(-1.f, 1.f, 1.f),
    		Point(1.f,  1.f, 1.f),
    		Point(1.f, -1.f, 1.f)
    	};

    	Plane plane(points[0], points[1], points[2]);
    	Point infront(0.f, 0.f, 0.5f);
    	Point coplanar(1.f, 1.f, 1.f);
    	Point behind(0.f, 0.f, 5.f);

    	if (plane.distanceTo(infront) > 0.f
    		&& plane.distanceTo(coplanar) == 0.f
    		&& plane.distanceTo(behind) < 0.f)
    	{
    		puts("Planes PASSED");
    	}
    	else
    	{
    		puts("Planes FAILED");
    	}
    }

    int main()
    {
    	TestMatrix();
    	TestPoint();
    	TestVector();
    	TestQuaternion();
    	TestPlane();
    	return 0;
    }

The zip file viewer built into the Developer Toolbox made use of the zlib library, as well as the zlibdll source additions.

Copyright 1999-2008 (C) FLIPCODE.COM and/or the original content author(s). All rights reserved.
Please read our Terms, Conditions, and Privacy information.