From 9b85799fb274f291d4fefd9de08c069fe11d46f1 Mon Sep 17 00:00:00 2001 From: Tavian Barnes Date: Sat, 13 Jun 2009 22:55:15 +0000 Subject: Add support for transformation matricies. --- libdimension/dimension/geometry.h | 55 ++++++++---- libdimension/geometry.c | 144 ++++++++++++++++++++++++++++++-- libdimensionxx/dimensionxx/geometry.hpp | 26 +++--- 3 files changed, 189 insertions(+), 36 deletions(-) diff --git a/libdimension/dimension/geometry.h b/libdimension/dimension/geometry.h index e1d1497..ca10163 100644 --- a/libdimension/dimension/geometry.h +++ b/libdimension/dimension/geometry.h @@ -25,24 +25,16 @@ #ifndef DIMENSION_GEOMETRY_H #define DIMENSION_GEOMETRY_H -/* Scalar and vector types. */ -typedef double dmnsn_scalar; -typedef struct { dmnsn_scalar x, y, z; } dmnsn_vector; +/* Vector and matrix types. */ -/* Shorthand for vector construction */ -dmnsn_vector dmnsn_vector_construct(dmnsn_scalar x, - dmnsn_scalar y, - dmnsn_scalar z); +typedef struct { double x, y, z; } dmnsn_vector; -/* Vector arithmetic */ - -dmnsn_vector dmnsn_vector_add(dmnsn_vector lhs, dmnsn_vector rhs); -dmnsn_vector dmnsn_vector_sub(dmnsn_vector lhs, dmnsn_vector rhs); -dmnsn_vector dmnsn_vector_mul(dmnsn_scalar lhs, dmnsn_vector rhs); -dmnsn_vector dmnsn_vector_div(dmnsn_vector lhs, dmnsn_scalar rhs); - -dmnsn_scalar dmnsn_vector_dot(dmnsn_vector lhs, dmnsn_vector rhs); -dmnsn_vector dmnsn_vector_cross(dmnsn_vector lhs, dmnsn_vector rhs); +typedef struct { + double n00, n01, n02, n03, + n10, n11, n12, n13, + n20, n21, n22, n23, + n30, n31, n32, n33; +} dmnsn_matrix; /* A line, or ray. */ typedef struct { @@ -50,7 +42,36 @@ typedef struct { dmnsn_vector n; /* A normal vector; the direction of the line */ } dmnsn_line; +/* Shorthand for vector/matrix construction */ + +dmnsn_vector dmnsn_vector_construct(double x, double y, double z); + +dmnsn_matrix dmnsn_matrix_construct(double a0, double a1, double a2, double a3, + double b0, double b1, double b2, double b3, + double c0, double c1, double c2, double c3, + double d0, double d1, double d2, double d3); +dmnsn_matrix dmnsn_translation_matrix(dmnsn_vector d); +/* theta/|theta| = axis, |theta| = angle */ +dmnsn_matrix dmnsn_rotation_matrix(dmnsn_vector theta); + +/* Vector and matrix arithmetic */ + +dmnsn_vector dmnsn_vector_add(dmnsn_vector lhs, dmnsn_vector rhs); +dmnsn_vector dmnsn_vector_sub(dmnsn_vector lhs, dmnsn_vector rhs); +dmnsn_vector dmnsn_vector_mul(double lhs, dmnsn_vector rhs); +dmnsn_vector dmnsn_vector_div(dmnsn_vector lhs, double rhs); + +double dmnsn_vector_dot(dmnsn_vector lhs, dmnsn_vector rhs); +dmnsn_vector dmnsn_vector_cross(dmnsn_vector lhs, dmnsn_vector rhs); + +double dmnsn_vector_norm(dmnsn_vector n); +dmnsn_vector dmnsn_vector_normalize(dmnsn_vector n); + +dmnsn_matrix dmnsn_matrix_mul(dmnsn_matrix lhs, dmnsn_matrix rhs); +dmnsn_vector dmnsn_matrix_vector_mul(dmnsn_matrix lhs, dmnsn_vector rhs); +dmnsn_line dmnsn_matrix_line_mul(dmnsn_matrix lhs, dmnsn_line rhs); + /* A point on a line, defined by x0 + t*n */ -dmnsn_vector dmnsn_line_point(dmnsn_line l, dmnsn_scalar t); +dmnsn_vector dmnsn_line_point(dmnsn_line l, double t); #endif /* DIMENSION_GEOMETRY_H */ diff --git a/libdimension/geometry.c b/libdimension/geometry.c index 0eab10b..548aae1 100644 --- a/libdimension/geometry.c +++ b/libdimension/geometry.c @@ -19,15 +19,78 @@ *************************************************************************/ #include "dimension.h" +#include /* Construct a vector from x, y, and z. Just for convienence. */ dmnsn_vector -dmnsn_vector_construct(dmnsn_scalar x, dmnsn_scalar y, dmnsn_scalar z) +dmnsn_vector_construct(double x, double y, double z) { dmnsn_vector v = { .x = x, .y = y, .z = z }; return v; } +/* Construct a matrix. */ +dmnsn_matrix +dmnsn_matrix_construct(double a0, double a1, double a2, double a3, + double b0, double b1, double b2, double b3, + double c0, double c1, double c2, double c3, + double d0, double d1, double d2, double d3) +{ + dmnsn_matrix m = { .n00 = a0, .n01 = a1, .n02 = a2, .n03 = a3, + .n10 = b0, .n11 = b1, .n12 = b2, .n13 = b3, + .n20 = c0, .n21 = c1, .n22 = c2, .n23 = c3, + .n30 = d0, .n31 = d1, .n32 = d2, .n33 = d3 }; + return m; +} + +/* Translation matrix */ +dmnsn_matrix +dmnsn_translation_matrix(dmnsn_vector d) +{ + return dmnsn_matrix_construct(1.0, 0.0, 0.0, d.x, + 0.0, 1.0, 0.0, d.y, + 0.0, 0.0, 1.0, d.z, + 0.0, 0.0, 0.0, 1.0); +} + +/* Rotation matrix; theta/|theta| = axis, |theta| = angle */ +dmnsn_matrix +dmnsn_rotation_matrix(dmnsn_vector theta) +{ + dmnsn_vector axis, n1, n2, n3; + double angle, s, t, x, y, z; + + axis = dmnsn_vector_normalize(theta); + angle = dmnsn_vector_norm(theta); + + /* Shorthand to fit logical lines on one line */ + + s = sin(angle); + t = 1.0 - cos(angle); + + x = axis.x; + y = axis.y; + z = axis.z; + + /* Construct vectors, then a matrix, so our dmnsn_matrix_construct() call + is reasonably small */ + + n1 = dmnsn_vector_construct(1.0 + t*(x*x - 1.0), + z*s + t*x*y, + -y*s + t*x*z); + n2 = dmnsn_vector_construct(-z*s + t*x*y, + 1.0 + t*(y*y - 1.0), + x*s + t*y*z); + n3 = dmnsn_vector_construct(y*s + t*x*z, + -x*s + t*y*z, + 1.0 + t*(z*z - 1.0)); + + return dmnsn_matrix_construct(n1.x, n2.x, n3.x, 0.0, + n1.y, n2.y, n3.y, 0.0, + n1.z, n2.z, n3.z, 0.0, + 0.0, 0.0, 0.0, 1.0); +} + /* Add two vectors */ dmnsn_vector dmnsn_vector_add(dmnsn_vector lhs, dmnsn_vector rhs) @@ -50,7 +113,7 @@ dmnsn_vector_sub(dmnsn_vector lhs, dmnsn_vector rhs) /* Multiply a vector by a scalar */ dmnsn_vector -dmnsn_vector_mul(dmnsn_scalar lhs, dmnsn_vector rhs) +dmnsn_vector_mul(double lhs, dmnsn_vector rhs) { dmnsn_vector v = { .x = lhs*rhs.x, .y = lhs*rhs.y, .z = lhs*rhs.z }; return v; @@ -58,14 +121,14 @@ dmnsn_vector_mul(dmnsn_scalar lhs, dmnsn_vector rhs) /* Divide a vector by a scalar */ dmnsn_vector -dmnsn_vector_div(dmnsn_vector lhs, dmnsn_scalar rhs) +dmnsn_vector_div(dmnsn_vector lhs, double rhs) { dmnsn_vector v = { .x = lhs.x/rhs, .y = lhs.y/rhs, .z = lhs.z/rhs }; return v; } /* Dot product */ -dmnsn_scalar +double dmnsn_vector_dot(dmnsn_vector lhs, dmnsn_vector rhs) { return lhs.x*rhs.x + lhs.y*rhs.y + lhs.z*rhs.z; @@ -81,9 +144,80 @@ dmnsn_vector_cross(dmnsn_vector lhs, dmnsn_vector rhs) return v; } +/* Length of vector */ +double +dmnsn_vector_norm(dmnsn_vector n) +{ + return sqrt(n.x*n.x + n.y*n.y + n.z*n.z); +} + +/* Normalized vector */ +dmnsn_vector +dmnsn_vector_normalize(dmnsn_vector n) +{ + return dmnsn_vector_div(n, dmnsn_vector_norm(n)); +} + +/* 4x4 matrix multiplication */ +dmnsn_matrix +dmnsn_matrix_mul(dmnsn_matrix lhs, dmnsn_matrix rhs) +{ + dmnsn_matrix r; + + r.n00 = lhs.n00*rhs.n00 + lhs.n01*rhs.n10 + lhs.n02*rhs.n20 + lhs.n03*rhs.n30; + r.n01 = lhs.n00*rhs.n01 + lhs.n01*rhs.n11 + lhs.n02*rhs.n21 + lhs.n03*rhs.n31; + r.n02 = lhs.n00*rhs.n02 + lhs.n01*rhs.n12 + lhs.n02*rhs.n22 + lhs.n03*rhs.n32; + r.n03 = lhs.n00*rhs.n03 + lhs.n01*rhs.n13 + lhs.n02*rhs.n23 + lhs.n03*rhs.n33; + + r.n10 = lhs.n10*rhs.n00 + lhs.n11*rhs.n10 + lhs.n12*rhs.n20 + lhs.n13*rhs.n30; + r.n11 = lhs.n10*rhs.n01 + lhs.n11*rhs.n11 + lhs.n12*rhs.n21 + lhs.n13*rhs.n31; + r.n12 = lhs.n10*rhs.n02 + lhs.n11*rhs.n12 + lhs.n12*rhs.n22 + lhs.n13*rhs.n32; + r.n13 = lhs.n10*rhs.n03 + lhs.n11*rhs.n13 + lhs.n12*rhs.n23 + lhs.n13*rhs.n33; + + r.n20 = lhs.n20*rhs.n00 + lhs.n21*rhs.n10 + lhs.n22*rhs.n20 + lhs.n23*rhs.n30; + r.n21 = lhs.n20*rhs.n01 + lhs.n21*rhs.n11 + lhs.n22*rhs.n21 + lhs.n23*rhs.n31; + r.n22 = lhs.n20*rhs.n02 + lhs.n21*rhs.n12 + lhs.n22*rhs.n22 + lhs.n23*rhs.n32; + r.n23 = lhs.n20*rhs.n03 + lhs.n21*rhs.n13 + lhs.n22*rhs.n23 + lhs.n23*rhs.n33; + + r.n30 = lhs.n30*rhs.n00 + lhs.n31*rhs.n10 + lhs.n32*rhs.n20 + lhs.n33*rhs.n30; + r.n31 = lhs.n30*rhs.n01 + lhs.n31*rhs.n11 + lhs.n32*rhs.n21 + lhs.n33*rhs.n31; + r.n32 = lhs.n30*rhs.n02 + lhs.n31*rhs.n12 + lhs.n32*rhs.n22 + lhs.n33*rhs.n32; + r.n33 = lhs.n30*rhs.n03 + lhs.n31*rhs.n13 + lhs.n32*rhs.n23 + lhs.n33*rhs.n33; + + return r; +} + +/* Affine transformation; lhs*(x,y,z,1), normalized so the fourth element is + 1 */ +dmnsn_vector +dmnsn_matrix_vector_mul(dmnsn_matrix lhs, dmnsn_vector rhs) +{ + dmnsn_vector r; + double w; + + r.x = lhs.n00*rhs.x + lhs.n01*rhs.y + lhs.n02*rhs.z + lhs.n03; + r.x = lhs.n10*rhs.x + lhs.n11*rhs.y + lhs.n12*rhs.z + lhs.n13; + r.x = lhs.n20*rhs.x + lhs.n21*rhs.y + lhs.n22*rhs.z + lhs.n23; + w = lhs.n30*rhs.x + lhs.n31*rhs.y + lhs.n32*rhs.z + lhs.n33; + + return dmnsn_vector_div(r, w); +} + +/* Affine line transformation; n = lhs*(x0 + n) - lhs*x0, x0 *= lhs */ +dmnsn_line +dmnsn_matrix_line_mul(dmnsn_matrix lhs, dmnsn_line rhs) +{ + dmnsn_line l; + l.x0 = dmnsn_matrix_vector_mul(lhs, rhs.x0); + l.n = dmnsn_vector_sub( + dmnsn_matrix_vector_mul(lhs, dmnsn_vector_add(rhs.x0, rhs.n)), + l.x0); + return l; +} + /* A point on a line, l. Returns l.x0 + t*l.n */ dmnsn_vector -dmnsn_line_point(dmnsn_line l, dmnsn_scalar t) +dmnsn_line_point(dmnsn_line l, double t) { return dmnsn_vector_add(l.x0, dmnsn_vector_mul(t, l.n)); } diff --git a/libdimensionxx/dimensionxx/geometry.hpp b/libdimensionxx/dimensionxx/geometry.hpp index 78b9b49..101d0e9 100644 --- a/libdimensionxx/dimensionxx/geometry.hpp +++ b/libdimensionxx/dimensionxx/geometry.hpp @@ -21,29 +21,27 @@ #ifndef DIMENSIONXX_GEOMETRY_HPP #define DIMENSIONXX_GEOMETRY_HPP -// Wrappers for geometric types (Scalars, Vectors, Lines (rays)). +// Wrappers for geometric types (Vectors, Matricies, Lines (rays)). #include namespace Dimension { - typedef dmnsn_scalar Scalar; // This is really `double' - // Wrapper for dmnsn_vector class Vector { public: Vector() { } - Vector(Scalar x, Scalar y, Scalar z) + Vector(double x, double y, double z) : m_vector(dmnsn_vector_construct(x, y, z)) { } explicit Vector(dmnsn_vector v) : m_vector(v) { } // Vector(const Vector& v); // ~Vector(); // Get the x, y, and z components. - Scalar x() const { return m_vector.x; } - Scalar y() const { return m_vector.y; } - Scalar z() const { return m_vector.z; } + double x() const { return m_vector.x; } + double y() const { return m_vector.y; } + double z() const { return m_vector.z; } // Vector arithmetic @@ -52,9 +50,9 @@ namespace Dimension { m_vector = dmnsn_vector_add(m_vector, rhs.m_vector); return *this; } Vector& operator-=(const Vector& rhs) { m_vector = dmnsn_vector_sub(m_vector, rhs.m_vector); return *this; } - Vector& operator*=(Scalar rhs) + Vector& operator*=(double rhs) { m_vector = dmnsn_vector_mul(rhs, m_vector); return *this; } - Vector& operator/=(Scalar rhs) + Vector& operator/=(double rhs) { m_vector = dmnsn_vector_div(m_vector, rhs); return *this; } // Get the wrapped vector @@ -80,7 +78,7 @@ namespace Dimension // line& operator=(const line& l); // Get the point `t' on the line (x0 + t*n) - Vector operator()(Scalar t) { return Vector(dmnsn_line_point(m_line, t)); } + Vector operator()(double t) { return Vector(dmnsn_line_point(m_line, t)); } // Get the wrapped line dmnsn_line dmnsn() const { return m_line; } @@ -108,7 +106,7 @@ namespace Dimension } inline Vector - operator*(const Vector& lhs, Scalar rhs) + operator*(const Vector& lhs, double rhs) { Vector r = lhs; r *= rhs; @@ -116,7 +114,7 @@ namespace Dimension } inline Vector - operator*(Scalar lhs, const Vector& rhs) + operator*(double lhs, const Vector& rhs) { Vector r = rhs; r *= lhs; @@ -124,7 +122,7 @@ namespace Dimension } inline Vector - operator/(const Vector& lhs, Scalar rhs) + operator/(const Vector& lhs, double rhs) { Vector r = lhs; r /= rhs; @@ -132,7 +130,7 @@ namespace Dimension } // Dot product - inline Scalar + inline double dot(const Vector& lhs, const Vector& rhs) { return dmnsn_vector_dot(lhs.dmnsn(), rhs.dmnsn()); -- cgit v1.2.3