math3d.cpp

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

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


float Math3d::ScalarProduct(const Vec3d &vector1, const Vec3d &vector2)
{
	return vector1.x * vector2.x + vector1.y * vector2.y +  vector1.z * vector2.z;
}

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

void Math3d::NormalizeVec(Vec3d &vec)
{
	const float length = sqrtf(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
	
	vec.x /= length;
	vec.y /= length;
	vec.z /= length;
}

float Math3d::Length(const Vec3d &vec)
{
	return sqrtf(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
}

float Math3d::SquaredLength(const Vec3d &vec)
{
	return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z;
}

float Math3d::Distance(const Vec3d &vector1, const Vec3d &vector2)
{
	const float x1 = vector1.x - vector2.x;
	const float x2 = vector1.y - vector2.y;
	const float x3 = vector1.z - vector2.z;

	return sqrtf(x1 * x1 + x2 * x2 + x3 * x3);
}

float Math3d::SquaredDistance(const Vec3d &vector1, const Vec3d &vector2)
{
	const float x1 = vector1.x - vector2.x;
	const float x2 = vector1.y - vector2.y;
	const float x3 = vector1.z - vector2.z;

	return x1 * x1 + x2 * x2 + x3 * x3;
}

float Math3d::Angle(const Vec3d &vector1, const Vec3d &vector2)
{
	const float sp = vector1.x * vector2.x + vector1.y * vector2.y + vector1.z * vector2.z;
	const float l1 = sqrtf(vector1.x * vector1.x + vector1.y * vector1.y + vector1.z * vector1.z);
	const float l2 = sqrtf(vector2.x * vector2.x + vector2.y * vector2.y + vector2.z * vector2.z);
	
	// added this. In some cases angle was numerically unstable
	float r = sp / (l1 * l2);
	if (r > 1.0) r = 1.0;
	if (r < -1.0) r = -1.0;
	return acosf(r);
}

float Math3d::EvaluateForm(const Vec3d &matrix1, const Mat3d &matrix2)
{
	const float t0 = matrix1.x * matrix2.r1 + matrix1.y * matrix2.r4 + matrix1.z * matrix2.r7;
	const float t1 = matrix1.x * matrix2.r2 + matrix1.y * matrix2.r5 + matrix1.z * matrix2.r8;
	const float t2 = matrix1.x * matrix2.r3 + matrix1.y * matrix2.r6 + matrix1.z * matrix2.r9;

	return t0 * matrix1.x + t1 * matrix1.y + t2 * matrix1.z;
}

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

void Math3d::Invert(const Mat3d &matrix, Mat3d &result)
{
	const float a = matrix.r1;
	const float b = matrix.r2;
	const float c = matrix.r3;
	const float d = matrix.r4;
	const float e = matrix.r5;
	const float f = matrix.r6;
	const float g = matrix.r7;
	const float h = matrix.r8;
	const float i = matrix.r9;

	float det_inverse = 1 / (-c * e * g + b * f * g + c * d * h - a * f * h - b * d * i + a * e * i);

	result.r1 = (-f * h + e * i) * det_inverse;
	result.r2 = (c * h - b * i) * det_inverse;
	result.r3 = (-c * e + b * f) * det_inverse;
	result.r4 = (f * g - d * i) * det_inverse;
	result.r5 = (-c * g + a * i) * det_inverse;
	result.r6 = (c * d - a * f) * det_inverse;
	result.r7 = (-e * g + d * h) * det_inverse;
	result.r8 = (b * g - a * h) * det_inverse;
	result.r9 = (-b * d + a * e) * det_inverse;
}

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

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

void Math3d::SubtractMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
{
	result.r1 = matrix1.r1 - matrix2.r1;
	result.r2 = matrix1.r2 - matrix2.r2;
	result.r3 = matrix1.r3 - matrix2.r3;
	result.r4 = matrix1.r4 - matrix2.r4;
	result.r5 = matrix1.r5 - matrix2.r5;
	result.r6 = matrix1.r6 - matrix2.r6;
	result.r7 = matrix1.r7 - matrix2.r7;
	result.r8 = matrix1.r8 - matrix2.r8;
	result.r9 = matrix1.r9 - matrix2.r9;
}

float Math3d::Det(Mat3d &matrix)
{
	return matrix.r1 * matrix.r5 * matrix.r9 
		+ matrix.r2 * matrix.r6 * matrix.r7
		+ matrix.r3 * matrix.r4 * matrix.r8
		- matrix.r3 * matrix.r5 * matrix.r7
		- matrix.r1 * matrix.r6 * matrix.r8
		- matrix.r2 * matrix.r4 * matrix.r9;
}


void Math3d::SetTransformation(Transformation3d &transformation, const Vec3d &rotation, const Vec3d &translation)
{
	Math3d::SetRotationMat(transformation.rotation, rotation);
	Math3d::SetVec(transformation.translation, translation);
}

void Math3d::SetTransformation(Transformation3d &transformation, const Transformation3d &sourceTransformation)
{
	Math3d::SetMat(transformation.rotation, sourceTransformation.rotation);
	Math3d::SetVec(transformation.translation, sourceTransformation.translation);
}

void Math3d::Invert(const Transformation3d &input, Transformation3d &result)
{
	Math3d::Invert(input.rotation, result.rotation);
	Math3d::MulMatVec(result.rotation, input.translation, result.translation);
	Math3d::MulVecScalar(result.translation, -1, result.translation);
}

void Math3d::MulTransTrans(const Transformation3d &transformation1, const Transformation3d &transformation2, Transformation3d &result)
{
	Math3d::MulMatVec(transformation1.rotation, transformation2.translation, transformation1.translation, result.translation);
	Math3d::MulMatMat(transformation1.rotation, transformation2.rotation, result.rotation);
}

void Math3d::MulTransVec(const Transformation3d &transformation, const Vec3d &vec, Vec3d &result)
{
	Math3d::MulMatVec(transformation.rotation, vec, transformation.translation, result);
}


void Math3d::MulQuatQuat(const Quaternion &quat1, const Quaternion &quat2, Quaternion &result)
{
	Vec3d v;
	
	CrossProduct(quat1.v, quat2.v, v);
	v.x += quat1.w * quat2.v.x + quat1.w * quat2.v.x;
	v.y += quat1.w * quat2.v.y + quat1.w * quat2.v.y;
	v.z += quat1.w * quat2.v.z + quat1.w * quat2.v.z;

	result.w = quat1.w * quat2.w - ScalarProduct(quat1.v, quat2.v);
	SetVec(result.v, v);
}


void Math3d::RotateVecQuaternion(const Vec3d &vec, const Vec3d &axis, float theta, Vec3d &result)
{
	/*const float st = sinf(theta * 0.5f);
	const float ct = cosf(theta * 0.5f);

	Quaternion a, q, r;
	Vec3d u;

	// u = normalized axis vector
	SetVec(u, axis);
	NormalizeVec(u);

	// set a
	SetVec(a.v, vec);
	a.w = 0;

	// set q
	q.v.x = st * u.x;
	q.v.y = st * u.y;
	q.v.z = st * u.z;
	q.w = ct;

	// calculate q * a
	MulQuatQuat(q, a, r);
	
	// calculate conjugate quaternion of q
	q.v.x = -q.v.x;
	q.v.y = -q.v.y;
	q.v.z = -q.v.z;

	// calculate q * a * q^
	MulQuatQuat(r, q, r);*/

	const float length = Length(axis);
	const float a = axis.x / length;
	const float b = axis.y / length;
	const float c = axis.z / length;
	const float d = theta;

	const float v1 = vec.x;
	const float v2 = vec.y;
	const float v3 = vec.z;

	const float t2 = a * b;
	const float t3 = a * c;
	const float t4 = a * d;
	const float t5 = -b * b;
	const float t6 = b * c;
	const float t7 = b * d;
	const float t8 = -c * c;
	const float t9 = c * d;
	const float t10 = -d * d;

	result.x = 2 * ((t8 + t10) * v1 + (t6 - t4) * v2 + (t3 + t7) * v3 ) + v1;
	result.y = 2 * ((t4 + t6) * v1 + (t5 + t10) * v2 + (t9 - t2) * v3 ) + v2;
	result.z = 2 * ((t7 - t3) * v1 + (t2 +  t9) * v2 + (t5 + t8) * v3 ) + v3;
}

void Math3d::RotateVecAngleAxis(const Vec3d &vec, const Vec3d &axis, float theta, Vec3d &result)
{
	Mat3d m;
	SetRotationMatAxis(m, axis, theta);
	MulMatVec(m, vec, result);
}

float Math3d::Angle(const Vec3d &vector1, const Vec3d &vector2, const Vec3d &axis)
{
	Vec3d u1, u2, n, temp;
	Mat3d testMatrix;

	Math3d::SetVec(n, axis);
	Math3d::NormalizeVec(n);

	Math3d::SetVec(u1, vector1);
	Math3d::MulVecScalar(n, Math3d::ScalarProduct(u1, n), temp);
	Math3d::SubtractFromVec(u1, temp);
	Math3d::NormalizeVec(u1);

	Math3d::SetVec(u2, vector2);
	Math3d::MulVecScalar(n, Math3d::ScalarProduct(u2, n), temp);
	Math3d::SubtractFromVec(u2, temp);
	Math3d::NormalizeVec(u2);
	
	// test which one of the two solutions is the right one
	Math3d::SetRotationMatAxis(testMatrix, n, Math3d::Angle(u1, u2));
	Math3d::MulMatVec(testMatrix, u1, temp);
	const float d1 = Math3d::Distance(temp, u2);

	Math3d::SetRotationMatAxis(testMatrix, n, -Math3d::Angle(u1, u2));
	Math3d::MulMatVec(testMatrix, u1, temp);
	const float d2 = Math3d::Distance(temp, u2);
	
	return d1 < d2 ? Math3d::Angle(u1, u2) : -Math3d::Angle(u1, u2);
}

void Math3d::GetAxisAndAngle(const Mat3d &R, Vec3d &axis, float &angle)
{
	const float x = R.r8 - R.r6;
	const float y = R.r3 - R.r7;
	const float z = R.r4 - R.r2;
	
	const float r = sqrtf(x * x + y * y + z * z);
	const float t = R.r1 + R.r5 + R.r9;
	
	angle = atan2(r, t - 1);
	
	Math3d::SetVec(axis, x, y, z);
}


void Math3d::GetNormalPosition( Vec3d n_Calibration , Vec3d v , Vec3d &res ) 
{
	//	Voldrem "ortogonalitzar" el vector vec en funci