math3d.cpp

This is a simple code sample to interpolate a 3d set of points using the bezier algorithm minimized

// *****************************************************************
// Filename:  Math3d.cpp

// Date:      2008
// *****************************************************************



// *****************************************************************
// includes
// *****************************************************************

#include "Math3d.h"
#include "Constants.h"

#include <math.h>




Vec3d Math3d::zero_vec = { 0, 0, 0 };
Mat3d Math3d::unit_mat = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
Mat3d Math3d::zero_mat = { 0, 0, 0, 0, 0, 0, 0, 0 ,0 };




void Math3d::SetVec(Vec3d &vec, float x, float y, float z)
{
	vec.x = x;
	vec.y = y;
	vec.z = z;
}

void Math3d::SetVec(Vec3d &vec, const Vec3d &sourceVector)
{
	vec.x = sourceVector.x;
	vec.y = sourceVector.y;
	vec.z = sourceVector.z;
}

void Math3d::SetMat(Mat3d &matrix, float r1, float r2, float r3, float r4, float r5, float r6, float r7, float r8, float r9)
{
	matrix.r1 = r1;
	matrix.r2 = r2;
	matrix.r3 = r3;
	matrix.r4 = r4;
	matrix.r5 = r5;
	matrix.r6 = r6;
	matrix.r7 = r7;
	matrix.r8 = r8;
	matrix.r9 = r9;
}

void Math3d::SetMat(Mat3d &matrix, const Mat3d &sourceMatrix)
{
	matrix.r1 = sourceMatrix.r1;
	matrix.r2 = sourceMatrix.r2;
	matrix.r3 = sourceMatrix.r3;
	matrix.r4 = sourceMatrix.r4;
	matrix.r5 = sourceMatrix.r5;
	matrix.r6 = sourceMatrix.r6;
	matrix.r7 = sourceMatrix.r7;
	matrix.r8 = sourceMatrix.r8;
	matrix.r9 = sourceMatrix.r9;
}

void Math3d::SetRotationMat(Mat3d &matrix, const Vec3d &rotation)
{
	const float alpha = rotation.x;
	const float beta = rotation.y;
	const float gamma = rotation.z;

	const float sinfalpha = sinf(alpha);
	const float cosfalpha = cosf(alpha);
	const float sinfbeta = sinf(beta);
	const float cosfbeta = cosf(beta);
	const float sinfgamma = sinf(gamma);
	const float cosfgamma = cosf(gamma);

	matrix.r1 = cosfbeta * cosfgamma;
	matrix.r2 = - cosfbeta * sinfgamma;
	matrix.r3 = sinfbeta;
	matrix.r4 = cosfalpha * sinfgamma + sinfalpha * sinfbeta * cosfgamma;
	matrix.r5 = cosfalpha * cosfgamma - sinfalpha * sinfbeta * sinfgamma;
	matrix.r6 = - sinfalpha * cosfbeta;
	matrix.r7 = sinfalpha * sinfgamma - cosfalpha * sinfbeta * cosfgamma;
	matrix.r8 = sinfalpha * cosfgamma + cosfalpha * sinfbeta * sinfgamma;
	matrix.r9 = cosfalpha * cosfbeta;
}

void Math3d::SetRotationMat(Mat3d &matrix, float alpha, float beta, float gamma)
{
	const float sinfalpha = sinf(alpha);
	const float cosfalpha = cosf(alpha);
	const float sinfbeta = sinf(beta);
	const float cosfbeta = cosf(beta);
	const float sinfgamma = sinf(gamma);
	const float cosfgamma = cosf(gamma);

	matrix.r1 = cosfbeta * cosfgamma;
	matrix.r2 = - cosfbeta * sinfgamma;
	matrix.r3 = sinfbeta;
	matrix.r4 = cosfalpha * sinfgamma + sinfalpha * sinfbeta * cosfgamma;
	matrix.r5 = cosfalpha * cosfgamma - sinfalpha * sinfbeta * sinfgamma;
	matrix.r6 = - sinfalpha * cosfbeta;
	matrix.r7 = sinfalpha * sinfgamma - cosfalpha * sinfbeta * cosfgamma;
	matrix.r8 = sinfalpha * cosfgamma + cosfalpha * sinfbeta * sinfgamma;
	matrix.r9 = cosfalpha * cosfbeta;
}

void Math3d::SetRotationMatYZX(Mat3d &matrix, const Vec3d &rotation)
{
	Mat3d temp;
	
	SetRotationMatY(matrix, rotation.y);
	
	SetRotationMatZ(temp, rotation.z);
	MulMatMat(temp, matrix, matrix);
	
	SetRotationMatX(temp, rotation.x);
	MulMatMat(temp, matrix, matrix);
}

void Math3d::SetRotationMatX(Mat3d &matrix, float theta)
{
	matrix.r1 = 1;
	matrix.r2 = matrix.r3 = matrix.r4 = matrix.r7 = 0;
	matrix.r5 = matrix.r9 = cosf(theta);
	matrix.r6 = matrix.r8 = sinf(theta);
	matrix.r6 = -matrix.r6;
}

void Math3d::SetRotationMatY(Mat3d &matrix, float theta)
{
	matrix.r5 = 1;
	matrix.r2 = matrix.r4 = matrix.r6 = matrix.r8 = 0;
	matrix.r1 = matrix.r9 = cosf(theta);
	matrix.r3 = matrix.r7 = sinf(theta);
	matrix.r7 = -matrix.r7;
}

void Math3d::SetRotationMatZ(Mat3d &matrix, float theta)
{
	matrix.r9 = 1;
	matrix.r3 = matrix.r6 = matrix.r7 = matrix.r8 = 0;
	matrix.r1 = matrix.r5 = cosf(theta);
	matrix.r2 = matrix.r4 = sinf(theta);
	matrix.r2 = -matrix.r2;
}

void Math3d::SetRotationMat(Mat3d &matrix, const Vec3d &axis, float theta)
{
	const float length = Length(axis);
	const float v1 = axis.x / length;
	const float v2 = axis.y / length;
	const float v3 = axis.z / length;

	const float t1 = cosf(theta);
	const float t2 = 1 - t1;
	const float t3 = v1 * v1;
	const float t6 = t2 * v1;
	const float t7 = t6 * v2;
	const float t8 = sinf(theta);
	const float t9 = t8 * v3;
	const float t11 = t6 * v3;
	const float t12 = t8 * v2;
	const float t15 = v2 * v2;
	const float t19 = t2 * v2 * v3;
	const float t20 = t8 * v1;
	const float t24 = v3 * v3;

	matrix.r1 = t1 + t2 * t3;
	matrix.r2 = t7 - t9;
	matrix.r3 = t11 + t12;
	matrix.r4 = t7 + t9;
	matrix.r5 = t1 + t2 * t15;
	matrix.r6 = t19 - t20;
	matrix.r7 = t11 - t12;
	matrix.r8 = t19 + t20;
	matrix.r9 = t1 + t2 * t24;
}

void Math3d::SetRotationMatAxis(Mat3d &matrix, const Vec3d &axis, float theta)
{
	const float length = Length(axis);
	const float x = axis.x / length;
	const float y = axis.y / length;
	const float z = axis.z / length;
	
	const float s = sinf(theta);
	const float c = cosf(theta);
	const float t = 1.0 - c;   
	
	matrix.r1 = t * x * x + c;
	matrix.r2 = t * x * y - s * z;
	matrix.r3 = t * x * z + s * y;	
	matrix.r4 = t * x * y + s * z;
	matrix.r5 = t * y * y + c;
	matrix.r6 = t * y * z - s * x;
	matrix.r7 = t * x * z - s * y;
	matrix.r8 = t * y * z + s * x;
	matrix.r9 = t * z * z + c;
}


void Math3d::MulMatVec(const Mat3d &matrix, const Vec3d &vec, Vec3d &result)
{
	const float x = vec.x;
	const float y = vec.y;
	const float z = vec.z;

 	result.x = matrix.r1 * x + matrix.r2 * y + matrix.r3 * z;
	result.y = matrix.r4 * x + matrix.r5 * y + matrix.r6 * z;
	result.z = matrix.r7 * x + matrix.r8 * y + matrix.r9 * z;
}

void Math3d::MulMatVec(const Mat3d &matrix, const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
{
	const float x = vector1.x;
	const float y = vector1.y;
	const float z = vector1.z;

	result.x = matrix.r1 * x + matrix.r2 * y + matrix.r3 * z + vector2.x;
	result.y = matrix.r4 * x + matrix.r5 * y + matrix.r6 * z + vector2.y;
	result.z = matrix.r7 * x + matrix.r8 * y + matrix.r9 * z + vector2.z;
}

void Math3d::MulMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
{
	const float x1 = matrix1.r1 * matrix2.r1 + matrix1.r2 * matrix2.r4 + matrix1.r3 * matrix2.r7;
	const float x2 = matrix1.r1 * matrix2.r2 + matrix1.r2 * matrix2.r5 + matrix1.r3 * matrix2.r8;
	const float x3 = matrix1.r1 * matrix2.r3 + matrix1.r2 * matrix2.r6 + matrix1.r3 * matrix2.r9;
	const float x4 = matrix1.r4 * matrix2.r1 + matrix1.r5 * matrix2.r4 + matrix1.r6 * matrix2.r7;
	const float x5 = matrix1.r4 * matrix2.r2 + matrix1.r5 * matrix2.r5 + matrix1.r6 * matrix2.r8;
	const float x6 = matrix1.r4 * matrix2.r3 + matrix1.r5 * matrix2.r6 + matrix1.r6 * matrix2.r9;
	const float x7 = matrix1.r7 * matrix2.r1 + matrix1.r8 * matrix2.r4 + matrix1.r9 * matrix2.r7;
	const float x8 = matrix1.r7 * matrix2.r2 + matrix1.r8 * matrix2.r5 + matrix1.r9 * matrix2.r8;
	const float x9 = matrix1.r7 * matrix2.r3 + matrix1.r8 * matrix2.r6 + matrix1.r9 * matrix2.r9;
	
	result.r1 = x1;
	result.r2 = x2;
	result.r3 = x3;
	result.r4 = x4;
	result.r5 = x5;
	result.r6 = x6;
	result.r7 = x7;
	result.r8 = x8;
	result.r9 = x9;
}

void Math3d::MulVecTransposedVec(const Vec3d &vector1, const Vec3d &vector2, Mat3d &result)
{
	result.r1 = vector1.x * vector2.x;
	result.r2 = vector1.x * vector2.y;
	result.r3 = vector1.x * vector2.z;
	result.r4 = vector1.y * vector2.x;
	result.r5 = vector1.y * vector2.y;
	result.r6 = vector1.y * vector2.z;
	result.r7 = vector1.z * vector2.x;
	result.r8 = vector1.z * vector2.y;
	result.r9 = vector1.z * vector2.z;
}


void Math3d::AddToVec(Vec3d &vec, const Vec3d &vectorToAdd)
{
	vec.x += vectorToAdd.x;
	vec.y += vectorToAdd.y;
	vec.z += vectorToAdd.z;
}

void Math3d::SubtractFromVec(Vec3d &vec, const Vec3d &vectorToSubtract)
{
	vec.x -= vectorToSubtract.x;
	vec.y -= vectorToSubtract.y;
	vec.z -= vectorToSubtract.z;
}

void Math3d::AddVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
{
	result.x = vector1.x + vector2.x;
	result.y = vector1.y + vector2.y;
	result.z = vector1.z + vector2.z;
}

void Math3d::MulVecScalar(const Vec3d &vec, float scalar, Vec3d &result)
{
	result.x = scalar * vec.x;
	result.y = scalar * vec.y;
	result.z = scalar * vec.z;
}

void Math3d::MulMatScalar(const Mat3d &matrix, float scalar, Mat3d &result)
{
	result.r1 = scalar * matrix.r1;
	result.r2 = scalar * matrix.r2;
	result.r3 = scalar * matrix.r3;
	result.r4 = scalar * matrix.r4;
	result.r5 = scalar * matrix.r5;
	result.r6 = scalar * matrix.r6;
	result.r7 = scalar * matrix.r7;
	result.r8 = scalar * matrix.r8;
	result.r9 = scalar * matrix.r9;
}

void Math3d::SubtractVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
{
	result.x = vector1.x - vector2.x;
	result.y = vector1.y - vector2.y;
	result.z = vector1.z - vector2.z;
}


void Math3d::RotateVec(const Vec3d &vec, const Vec3d &rotation, Vec3d &result)
{
    Mat3d matrix;
	SetRotationMat(matrix, rotation);
	MulMatVec(matrix, vec, result);
}

void Math3d::TransformVec(const Vec3d &vec, const Vec3d &rotation, const Vec3d &translation, Vec3d &result)
{
    Mat3d matrix;
	SetRotationMat(matrix, rotation);
	MulMatVec(matrix, vec, translation, result);
}

void Math3d::RotateVecYZX(const Vec3d &vec, const Vec3d &rotation, Vec3d &result)
{
    Mat3d matrix;
	SetRotationMatYZX(matrix, rotation);
	MulMatVec(matrix, vec, result);
}