From 90ce63bcd81f9f2f0faf5ce8f27fc8dc249d8390 Mon Sep 17 00:00:00 2001
From: Amorilia <amorilia@users.sourceforge.net>
Date: Fri, 9 Dec 2005 22:18:54 +0000
Subject: [PATCH] Added a function to calculate an X-aligned bone matrix from
the bone vector and the bone roll, which is convenient to have for the
Blender exporter.
---
nif_math.cpp | 82 +++++++++++++++++++++++++++++++++++++++++++++++++++-
nif_math.h | 6 ++++
niflib.h | 2 ++
3 files changed, 89 insertions(+), 1 deletion(-)
diff --git a/nif_math.cpp b/nif_math.cpp
index 64f9658f..c1bf2b61 100644
--- a/nif_math.cpp
+++ b/nif_math.cpp
@@ -33,13 +33,21 @@ POSSIBILITY OF SUCH DAMAGE. */
#include "nif_math.h"
-void PrintMatrix( Matrix33 const & m, ostream & out ) {
+void PrintMatrix33( Matrix33 const & m, ostream & out ) {
out << endl
<< " |" << setw(8) << m[0][0] << "," << setw(8) << m[0][1] << "," << setw(8) << m[0][2] << " |" << endl
<< " |" << setw(8) << m[1][0] << "," << setw(8) << m[1][1] << "," << setw(8) << m[1][2] << " |" << endl
<< " |" << setw(8) << m[2][0] << "," << setw(8) << m[2][1] << "," << setw(8) << m[2][2] << " |" <<endl;
}
+void PrintMatrix44( Matrix44 const & m, ostream & out ) {
+ out << endl
+ << " |" << setw(8) << m[0][0] << "," << setw(8) << m[0][1] << "," << setw(8) << m[0][2] << setw(8) << m[0][3] << " |" << endl
+ << " |" << setw(8) << m[1][0] << "," << setw(8) << m[1][1] << "," << setw(8) << m[1][2] << setw(8) << m[1][3] << " |" << endl
+ << " |" << setw(8) << m[2][0] << "," << setw(8) << m[2][1] << "," << setw(8) << m[2][2] << setw(8) << m[2][3] << " |" << endl
+ << " |" << setw(8) << m[3][0] << "," << setw(8) << m[3][1] << "," << setw(8) << m[3][2] << setw(8) << m[3][3] << " |" << endl;
+}
+
Vector3 MultVector3( Vector3 const & a, Vector3 const & b ) {
Vector3 answer;
answer.x = a.y * b.z - a.z * b.y;
@@ -290,3 +298,75 @@ void QuatToEuler( Quaternion const & quat, ostream & out ) {
<< " Bank: " << b << endl;
}
+
+float Length(Vector3 const & a) {
+ return sqrt(a.x*a.x + a.y*a.y + a.z*a.z);
+}
+
+Vector3 Normalize(Vector3 const & a) {
+ float l = Length(a);
+ return Vector3(a.x/l, a.y/l, a.z/l);
+}
+
+float DotProduct(Vector3 const & a, Vector3 const & b) {
+ return a.x*b.x + a.y*b.y + a.z*b.z;
+}
+
+Vector3 CrossProduct(Vector3 const & a, Vector3 const & b) {
+ return Vector3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
+}
+
+float Angle(Vector3 const & a, Vector3 const & b) {
+ float pi = 3.141592653589793f;
+ float s = Length(a) * Length(b);
+ float f = DotProduct(a, b) / s;
+ if (f >= 1.0f) return 0.0f;
+ if (f <= -1.0f) return pi / 2.0f;
+ return atan(-f / sqrt(1.0f - f * f)) + pi / 2.0f;
+}
+
+Matrix44 RotateMatrix44( Vector3 const & axis, float angle) {
+ float vx = axis.x;
+ float vy = axis.y;
+ float vz = axis.z;
+ float vx2 = vx * vx;
+ float vy2 = vy * vy;
+ float vz2 = vz * vz;
+ float cs = cos(angle);
+ float sn = sin(angle);
+ float co1 = 1.0f - cs;
+ return Matrix44(
+ vx2 * co1 + cs, vx * vy * co1 + vz * sn, vz * vx * co1 - vy * sn, 0.0f,
+ vx * vy * co1 - vz * sn, vy2 * co1 + cs, vy * vz * co1 + vx * sn, 0.0f,
+ vz * vx * co1 + vy * sn, vy * vz * co1 - vx * sn, vz2 * co1 + cs, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f);
+}
+
+Matrix44 BoneToMatrix44( Vector3 const & bone_vec, float roll ) {
+ float theta;
+ Matrix44 bMatrix, rMatrix, result;
+
+ Vector3 target(1.0f, 0.0f, 0.0f);
+ Vector3 nor = Normalize(bone_vec);
+ Vector3 axis = CrossProduct(target, nor);
+ if (DotProduct(axis, axis) > 0.0000000000001) {
+ axis = Normalize(axis);
+ theta = acos(DotProduct(target, nor));
+ bMatrix = RotateMatrix44(axis, theta);
+ } else {
+ float updown;
+ if (DotProduct(target, nor) > 0.0) updown = 1.0f;
+ else updown = -1.0f;
+ bMatrix = Matrix44(
+ updown, 0.0f, 0.0f, 0.0f,
+ 0.0f, updown, 0.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f);
+ };
+ rMatrix = RotateMatrix44(nor, roll);
+ result = MultMatrix44(bMatrix, rMatrix);
+ result[3][0] = bone_vec.x;
+ result[3][1] = bone_vec.y;
+ result[3][2] = bone_vec.z;
+ return result;
+}
diff --git a/nif_math.h b/nif_math.h
index b07db031..52504446 100644
--- a/nif_math.h
+++ b/nif_math.h
@@ -56,6 +56,12 @@ Matrix44 InverseMatrix44( Matrix44 const & m );
void SetIdentity33( Matrix33 & m );
void SetIdentity44( Matrix44 & m );
void PrintMatrix33( Matrix33 const & m, ostream & out );
+void PrintMatrix44( Matrix44 const & m, ostream & out );
Quaternion MatrixToQuat( Matrix33 const & m );
+Vector3 Normalize(Vector3 const & a);
+float DotProduct(Vector3 const & a, Vector3 const & b);
+Vector3 CrossProduct(Vector3 const & a, Vector3 const & b); // = MultVector3
+float Angle(Vector3 const & a, Vector3 const & b);
+Matrix44 BoneToMatrix44( Vector3 const & bone_vec, float roll );
#endif
diff --git a/niflib.h b/niflib.h
index 486e59cb..f4e2460a 100644
--- a/niflib.h
+++ b/niflib.h
@@ -425,6 +425,8 @@ struct Matrix44 {
}
};
+// Bone calculation helper function from the nif_math module.
+Matrix44 BoneToMatrix44( Vector3 const & bone_vec, float roll );
struct Color {
float r, g, b, a;
--
GitLab