From ee5049f8cdb9dc8e4d66cad64b08efc8f2ed8cb9 Mon Sep 17 00:00:00 2001 From: RCmags Date: Sun, 4 Dec 2022 12:58:27 -0400 Subject: [PATCH] downgrade to v1.0.1 --- README.md | 23 +- examples/quaternion/quaternion.ino | 347 ++++++++++---------- examples/vector/vector.ino | 217 ++++++------- keywords.txt | 17 +- library.properties | 4 +- src/quaternion_type.cpp | 498 +++++++++++++++-------------- src/quaternion_type.h | 56 ++-- src/vector_type.cpp | 275 ++++++++-------- src/vector_type.h | 13 +- 9 files changed, 702 insertions(+), 748 deletions(-) diff --git a/README.md b/README.md index e8d9993..74e27cf 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,9 @@ -# vector_datatype -This library includes structs for 3d vectors and quaternions. Both vectors and quaternions can perform basic operations such as addition, subtraction, multiplication, division, increment, decrement, etc. Other operations include the dot product, cross product, magnitude, and normalize. - -Quaternions can also be defined as unit quaternions to represent rotations and can be used to rotate and scale vectors. Vectors are compatible with quaternions, and both types are compatible with arrays. A length-3 array can be added, subtracted, etc, from a vector or a quaternion, and a length-4 array can perform the same operations with a quaternion. See the examples for a complete list of the operations available for each type. - -For more information on vector and quaternion operations, see these links: - -- [Quaternions and rotations](http://danceswithcode.net/engineeringnotes/quaternions/quaternions.html) -- [Quaternion operations](http://graphics.stanford.edu/courses/cs348a-17-winter/Papers/quaternion.pdf) -- [Vector operations](http://emweb.unl.edu/Math/mathweb/vectors/vectors.html) - -![alt text](http://danceswithcode.net/engineeringnotes/quaternions/images/axis-angle.png) -![alt text](https://opentextbc.ca/calculusv3openstax/wp-content/uploads/sites/288/2019/08/CNX_Calc_Figure_12_02_011.jpg) - +# vector_datatype +This is a library for 3d vectors and quaternions. It includes two structs to define each datatype. Both vectors and quaternions can perform basic operations such as addition, subtraction, multiplication, division, increment, decrement, and their analogs for multiplication and division. Vectors and quaternions are also compatible with each other. + +Included are other operations such as vector dot product and cross product. Quaternions can also be constructed as unit quaternions to represent rotations. They can also rotate vectors. Both datatypes are compatible with arrays, meaning that a length-3 array can be added, subtracted, etc from a vector. This also applies to quaternions. + +For more information on vector and quaternion operations, see these links: + +- [Quaternion operations](http://graphics.stanford.edu/courses/cs348a-17-winter/Papers/quaternion.pdf) +- [Vector operations](http://emweb.unl.edu/Math/mathweb/vectors/vectors.html) diff --git a/examples/quaternion/quaternion.ino b/examples/quaternion/quaternion.ino index 4efdbb3..ff4f43b 100644 --- a/examples/quaternion/quaternion.ino +++ b/examples/quaternion/quaternion.ino @@ -1,184 +1,163 @@ -/* - This sketch shows to perform operations on quaternions and how to convert arrays to quaternions. - */ - -#include - -//======= Display functions ======= - -void printQuat( vec3_t vec ) { - Serial.print( vec.x ); - Serial.print( ", " ); - Serial.print( vec.y ); - Serial.print( ", " ); - Serial.print( vec.z ); -} - -void printQuat( quat_t quat ) { - Serial.print( quat.w ); - Serial.print( ", " ); - printQuat( quat.v ); -} - -template -void printOperation(String label, T input) { - Serial.println(label); - Serial.print( "\t\t(" ); - printQuat(input); - Serial.print( ")\n\n" ); -} - -//========== Main program ========= - -void setup() { - Serial.begin(9600); - Serial.flush(); - - //-------- Quaternions -------- - quat_t q1 = { 1, 0, 0, 0 }; - quat_t q2 = { 0, 1, 0, 0 }; - quat_t q3 = { 0, 0, 1, 0 }; - quat_t q4 = { 0, 0, 0, 1 }; - quat_t q5; - - // 1. Basic operations: - // Quaternions can be manipulated in the same way as scalar values - - // Addition - q5 = q1 + q2; - printOperation( "1. Addition: ", q5 ); - - // Subtraction - q5 = q2 - q3; - printOperation( "2. Subtraction: ", q5 ); - - // Negation: - q5 = -q5; - printOperation( "3. Negation: ", q5 ); - - // Increment: - q5 += q4; - printOperation( "4. Increment: ", q5 ); - - // Decrement: - q5 -= q1; - printOperation( "5. Decrement: ", q5 ); - - // Scalar product: - q5 = q5*5; - printOperation( "6A. Scalar product: ", q5 ); - // Reverse order - q5 = -3*q5; - printOperation( "6B. Scalar product (reverse order): ", q5 ); - - // Scalar divide: - q5 = q5/8; - printOperation( "7. Scalar divide: ", q5 ); - - // Scalar self multiply: - q5 *= 10; - printOperation( "8. Scalar self multiply: ", q5 ); - - // Scalar self divide: - q5 /= 2; - printOperation( "9. Scalar self divide: ", q5 ); - - // 2. Quaternion multiplication and division: - /* Quats can be multiplied together to generate a new quaternion. This can encode multiple rotations */ - - // Multiply - q5 = q5*q1; - printOperation( "10. Quaternion multiply: ", q5 ); - - // Divide - q5 = q5/q2; - printOperation( "11. Quaternion division: ", q5 ); - - // Self multiply - q5 *= q3; - printOperation( "12. Quaternion self multiply: ", q5 ); - - // Self divide - q5 /= q4; - printOperation( "13. Quaternion self divide: ", q5 ); - - // 3. Important operations: - - // Conjugate - printOperation( "14. Conjugate: ", q5.conj() ); - - // Normalize - printOperation( "15. Normalize: ", q5.norm() ); - - // Inner product - Serial.print( "16. Inner product: " ); - Serial.println( q5.inner() ); - - // Magnitude - Serial.print( "17. Magnitude: " ); - Serial.print( q5.mag() ); - Serial.print("\n\n"); - - // 4. Compatibility with arrays: - /* In operations between a quaternion and an array, the array will be interpreted as a quaternion */ - float arr[] = {1, 2, 3, 4}; - - printOperation( "18. Quaternion and array addition: " , q5 + arr ); - printOperation( "19. Quaternion and array subtraction: ", q5 - arr ); - printOperation( "20. Quaternion and array multiply: " , q5 * arr ); - - // Operations with arrays are not converted to a quaternion type. These need to be explicitly converted. - printOperation( "21. Array conversion (multiply): ", quat_t(arr)*3 ); - printOperation( "22. Array conversion (divide): " , quat_t(arr)/10 ); - - //-- Compatibility with vectors - /* Vectors will be interpreted as quaternions without a scalar value (w = 0) */ - vec3_t vec = {1, 2, 3}; - - printOperation( "23. Quaternion and vector addition: " , q5 + vec ); - printOperation( "24. Quaternion and vector subtraction: ", q5 - vec ); - printOperation( "25. Quaternion and vector multiply; " , q5 * vec ); - - // 5. Vector rotation: a quaternion can rotate and stretch a 3D vector - - // A) Rotate by angle about axis - constexpr float angle = (PI/180.0) * 30; - vec3_t axis = { 0, 1, 0 }; - - /* Can choose between LARGE_ANGLE and SMALL_ANGLE. - * Use SMALL_ANGLE for a small angle approximation (less than 30 deg) and faster excecution */ - quat_t qrot; qrot.setRotation( axis, angle, SMALL_ANGLE ); - vec3_t vec_rot; // rotated vector - vec = { 1,0,0 }; // initial vector - - // Rotate into angle - vec_rot = qrot.rotate(vec, GLOBAL_FRAME); - - printOperation( "26. Rotate by axis and angle (Global): ", vec_rot ); - - // Rotate away from angle - vec_rot = qrot.rotate(vec, LOCAL_FRAME); - - printOperation( "27. Rotate by axis and angle (local): ", vec_rot ); - - // B) Rotate by unit vector of magnitude sin(angle) - qrot.setRotation( axis, LARGE_ANGLE ); - vec_rot = qrot.rotate(vec, GLOBAL_FRAME); - - printOperation( "28. Rotate by unit vector: ", vec_rot ); - - // 6. Axes projections: can choose between GLOBAL_FRAME and LOCAL_FRAME - /* GLOBAL_FRAME projects local axis-vectors to global coordinates - * LOCAL_FRAME projects global axis-vectors to local coordinates */ - qrot.setRotation( axis, angle, LARGE_ANGLE ); - - vec3_t x = qrot.axisX(GLOBAL_FRAME); - vec3_t y = qrot.axisY(GLOBAL_FRAME); - vec3_t z = qrot.axisZ(GLOBAL_FRAME); - - printOperation( "29. X-axis vector: ", x ); - printOperation( "30. Y-axis vector: ", y ); - printOperation( "31. Z-axis vector: ", z ); -} - -void loop() {} +/* + This sketch shows to perform operations on quaternions and how to convert arrays to quaternions. + */ + +#include + +// Display functions + +void printVector( vec3_t vec ) { + Serial.print( vec.x ); + Serial.print( ", " ); + Serial.print( vec.y ); + Serial.print( ", " ); + Serial.print( vec.z ); + Serial.println(); +} + +void printQuat( quat_t quat ) { + Serial.print( quat.w ); + Serial.print( ", " ); + printVector( quat.v ); +} + +void setup() { + Serial.begin(9600); + Serial.flush(); + + //-------- Quaternions -------- + quat_t q1 = { 1, 0, 0, 0 }; + quat_t q2 = { 0, 1, 0, 0 }; + quat_t q3 = { 0, 0, 1, 0 }; + quat_t q4 = { 0, 0, 0, 1 }; + quat_t q5; + + //-- Operations: + // Quaternions can be manipulated in the same way as scalar values + + // Addition + q5 = q1 + q2; + printQuat( q5 ); + + // Subtraction + q5 = q5 - q3; + printQuat( q5 ); + + // Negation: + q5 = -q5; + printQuat( q5 ); + + // Increment: + q5 += q4; + printQuat( q5 ); + + // Decrement: + q5 -= q1; + printQuat( q5 ); + + // Scalar product: + q5 = q5*5; + printQuat( q5 ); + // Reverse order + q5 = -3*q5; + printQuat( q5 ); + + // Scalar divide: + q5 = q5/8; + printQuat( q5 ); + + // Scalar self multiply: + q5 *= 10; + printQuat( q5 ); + + // Scalar self divide: + q5 /= 20; + printQuat( q5 ); + + //- Important operations: + // These are useful operations that are commonly encountered. They are explicitly named for clarity. + + // Conjugate + printQuat( q5.conj() ); + + // Inner product + Serial.println( q5.inner() ); + + // Magnitude + Serial.println( q5.mag() ); + + // Normalize + printQuat( q5.norm() ); + + //- Quaternion multiplication and division: + // Quats can be multiplied together to generate a new quaternion. This can encode simultaneous rotations. + + // Multiply + q5 = q5*q2; + printQuat( q5 ); + + // Divide + q5 = q5/q2; + printQuat( q5 ); + + // Self multiply + q5 *= q4; + printQuat( q5 ); + + // Self divide + q5 /= q3; + printQuat( q5 ); + + //- Vector rotation: + // A quaternion can rotate and stretch a 3D vector + + // A) Rotate by angle about axis + constexpr float angle = (PI/180.0) * 30; + vec3_t axis = { 0, 1, 0 }; + + quat_t qrot; + qrot.setRotation( false, axis, angle ); // Unit quaternion: true = small angle approximation ; false = exact rotation + vec3_t vec = { 1,0,0 }; // Vector to rotate + + // Rotate into angle + printQuat( qrot.rotate(true, vec) ); + // Rotate away from angle + printQuat( qrot.rotate(false, vec) ); + + // Axes projections + printVector( qrot.axisX(true) ); + printVector( qrot.axisY(true) ); + printVector( qrot.axisZ(true) ); + + // B) Rotate by unit vector + qrot.setRotation( false, axis ); // Unit vector has magniude sin(angle) + + printQuat( qrot.rotate(true, vec) ); + + //-- Compatibility with arrays: + // In operations between a quaternion and an array, the array will be interpreted as a quaternion. + + float arr[] = {1, 2, 3, 4}; + + printQuat( qrot + arr ); + printQuat( qrot - arr ); + printQuat( qrot*arr ); + + // Operations with arrays are not converted to a quaternion type. These need to be explicitly converted. + printQuat( quat_t(arr)*3 ); + printQuat( quat_t(arr)/10 ); + + printQuat( qrot.rotate(true, vec) ); + + //-- Compatibility with vectors + // Vectors will be interpreted as quaternions without a scalar value (w = 0) + + vec3_t v = {1, 2, 3}; + quat_t q6 = {1, 3, 3, 3}; + + printQuat( q6 + v ); + printQuat( q6 - v ); + printQuat( q6*v ); +} + +void loop() {} diff --git a/examples/vector/vector.ino b/examples/vector/vector.ino index 367628e..7cfb0ca 100644 --- a/examples/vector/vector.ino +++ b/examples/vector/vector.ino @@ -1,116 +1,101 @@ -/* - This sketch shows to perform operations on vectors and how to convert arrays to vectors. - */ - -#include - -//======= Display functions ======= - -void printVector( vec3_t vec ) { - Serial.print( vec.x ); - Serial.print( ", " ); - Serial.print( vec.y ); - Serial.print( ", " ); - Serial.print( vec.z ); -} - -void printOperation(String label, vec3_t input) { - Serial.println(label); - Serial.print( "\t\t(" ); - printVector(input); - Serial.print( ")\n\n" ); -} - -//========== Main program ========= - -void setup() { - Serial.begin(9600); - Serial.flush(); - - //-------- Vectors -------- - vec3_t v1 = { 1, 0, 0 }; - vec3_t v2 = { 0, 1, 0 }; - vec3_t v3 = { 0, 0, 1 }; - vec3_t v4; - - // 1. Operations: - // Vectors can be manipulated in the same way as scalar values - - // Addition - v4= v1 + v2; - printOperation( "1. Addition: ", v4 ); - - // Subtraction - v4 = v4 - v3; - printOperation( "2. Subtraction: ", v4 ); - - // Negation: - v4 = -v4; - printOperation( "3. Negation: ", v4 ); - - // Increment: - v4 += v2; - printOperation( "4. Increment: ", v4 ); - - // Decrement: - v4 -= v1; - printOperation( "5. Decrement: ", v4 ); - - // Scalar product: - v4 = v4*5; - printOperation( "6A. Scalar product: ", v4 ); - - // Reverse order - v4 = -3*v4; - printOperation( "6B. Scalar product (reverse order): ", v4 ); - - // Scalar divide: - v4 = v4/8; - printOperation( "7. Scalar divide: ", v4 ); - - // Self multiply: - v4 *= 10; - printOperation( "8. Self multiply: ", v4 ); - - // Self divide: - v4 /= 20; - printOperation( "9. Self divide: ", v4 ); - - // 2. Important operations: - // These are useful operations that are commonly encountered. They are explicitly named for clarity. - - // Normalize - printOperation( "10. Normalize vector: ", v4.norm() ); - - // Cross product - v4 = v4.cross(v1); - printOperation( "11. Cross product: ", v4 ); - - // Dot product - Serial.print( "11. Dot product: " ); - Serial.println( v4.dot(v2) ); - - // Magnitude - Serial.print( "12. Magnitude: " ); - Serial.print( v4.mag() ); - Serial.println("\n"); - - // 3. Compatibility with arrays: - // In operations between a vector and an array, the array will be interpreted as a vector. - - float arr[] = {1, 2, 3}; - - printOperation( "13. Vector and array addition: " , v4 + arr ); - printOperation( "14. Vector and array subtraction: " , v4 - arr ); - printOperation( "15. Vector and array cross product: ", v4.cross(arr) ); - - Serial.print( "16. Dot product (array): " ); - Serial.print( v4.dot(arr) ); - Serial.println("\n"); - - // Operations with arrays are not converted to a vector type. These need to be explicitly converted. - printOperation( "17. Array conversion (multiply): ", vec3_t(arr)*3 ); - printOperation( "18. Array conversion (divide): ", vec3_t(arr)/10 ); -} - -void loop() {} +/* + This sketch shows to perform operations on vectors and how to convert arrays to vectors. + */ + +#include + +// Display function +void printVector( vec3_t vec ) { + Serial.print( vec.x ); + Serial.print( ", " ); + Serial.print( vec.y ); + Serial.print( ", " ); + Serial.print( vec.z ); + Serial.println(); +} + +void setup() { + Serial.begin(9600); + Serial.flush(); + + //-------- Vectors -------- + vec3_t v1 = { 1, 0, 0 }; + vec3_t v2 = { 0, 1, 0 }; + vec3_t v3 = { 0, 0, 1 }; + vec3_t v4; + + //-- Operations: + // Vectors can be manipulated in the same way as scalar values + + // Addition + v4= v1 + v2; + printVector( v4 ); + + // Subtraction + v4 = v4 - v3; + printVector( v4 ); + + // Negation: + v4 = -v4; + printVector( v4 ); + + // Increment: + v4 += v2; + printVector( v4 ); + + // Decrement: + v4 -= v1; + printVector( v4 ); + + // Scalar product: + v4 = v4*5; + printVector( v4 ); + + // Reverse order + v4 = -3*v4; + printVector( v4 ); + + // Scalar divide: + v4 = v4/8; + printVector( v4 ); + + // Self multiply: + v4 *= 10; + printVector( v4 ); + + // Self divide: + v4 /= 20; + printVector( v4 ); + + //-- Important operations: + // These are useful operations that are commonly encountered. They are explicitly named for clarity. + + // Dot product + Serial.println( v4.dot(v1) ); + + // Cross product + v4 = v4.cross(v1); + printVector( v4 ); + + // Magnitude + Serial.println( v4.mag() ); + + // Normalize + printVector( v4.norm() ); + + //-- Compatibility with arrays: + // In operations between a vector and an array, the array will be interpreted as a vector. + + float arr[] = {1, 2, 3}; + + printVector( v4 + arr ); + printVector( v4 - arr ); + printVector( v4.cross(arr) ); + Serial.println( v4.dot(arr) ); + + // Operations with arrays are not converted to a vector type. These need to be explicitly converted. + printVector( vec3_t(arr)*3 ); + printVector( vec3_t(arr)/10 ); +} + +void loop() {} diff --git a/keywords.txt b/keywords.txt index a0cb355..6dc06c6 100644 --- a/keywords.txt +++ b/keywords.txt @@ -1,15 +1,2 @@ -# libraries -vector_type KEYWORD1 -quaternion_type KEYWORD1 - -# types -quat_t LITERAL1 -vec3_t LITERAL1 - -# constants -GLOBAL_FRAME LITERAL1 -LOCAL_FRAME LITERAL1 -SMALL_ANGLE LITERAL1 -LARGE_ANGLE LITERAL1 - -# NOTE: seperate keywords with single tab +vector_type KEYWORD1 +quaternion_type KEYWORD1 \ No newline at end of file diff --git a/library.properties b/library.properties index 473dc0d..d7751cb 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=Vector datatype -version=1.1.1 +version=1.0.1 author=RCmags maintainer=RCmags sentence=Library for 3d vectors and quaternions -paragraph=This library adds types for 3D vectors and 4D quaternions. It includes basic operations (add, multiply, etc) for both types, vector rotations for quaternions, and compatibility with arrays of 3 or 4 elements. +paragraph=Simple library for 3D vectors and quaternions. All basic operations are included along with rotations. Compabible with arrays. category=Other url=https://github.com/RCmags/vector_datatype architectures=* diff --git a/src/quaternion_type.cpp b/src/quaternion_type.cpp index 9413a92..14be982 100644 --- a/src/quaternion_type.cpp +++ b/src/quaternion_type.cpp @@ -1,246 +1,252 @@ -#include "quaternion_type.h" - -//------------------- Constructors ------------------- - -quat_t::quat_t() {} - -// Individual components -quat_t::quat_t( float _w, float _x, float _y, float _z ) { - w = _w; - v = { _x, _y, _z }; -} - -// Scalar and vector components -quat_t::quat_t( float _w, vec3_t _v ) { - w = _w; - v = _v; -} - -// Convert to vector -quat_t::quat_t( vec3_t _v ) { - w = 0; - v = _v; -} - -// Convert from array -quat_t::quat_t( float arr[] ) { - w = arr[0]; - v.x = arr[1]; - v.y = arr[2]; - v.z = arr[3]; -} - -//----------------- Basic Operations ------------------ - -//-- Addition and subtraction: - -// Addition -quat_t quat_t::operator + ( const quat_t &r ) { - quat_t q = { w + r.w , - v + r.v }; - return q; -} - -// Subtraction -quat_t quat_t::operator - ( const quat_t &r ) { - quat_t q = { w - r.w , - v - r.v }; - return q; -} -// Negation -quat_t quat_t::operator - ( void ) { - quat_t q = { -w , - -v }; - return q; -} - -// Increment -void quat_t::operator += ( const quat_t &r ) { - w += r.w; - v += r.v; -} -// Decrement -void quat_t::operator -= ( const quat_t &r ) { - w -= r.w; - v -= r.v; -} - -//-- Scalar product and division: - -// Scalar product -quat_t quat_t::operator * ( const float s ) { - quat_t q = { w * s , - v * s }; - return q; -} - -// Scalar division -quat_t quat_t::operator / ( const float s ) { - quat_t q = { w / s , - v / s }; - return q; -} - -// Self scalar multiply -void quat_t::operator *= ( const float s ) { - w *= s; - v *= s; -} - -// Self scalar divide -void quat_t::operator /= ( const float s ) { - w /= s; - v /= s; -} - -//-- Quaternion multiplication and division: - -// Multiplication -quat_t quat_t::operator * ( const quat_t &r ) { - quat_t q = { w*r.w - v.dot(r.v) , - w*r.v + v*r.w + v.cross(r.v) }; - return q; -} - -// Division -quat_t quat_t::operator / ( quat_t &r ) { - quat_t q = { w, v }; - return ( q*r.conj() )/r.inner(); -} - -// Self multiply -void quat_t::operator *= ( const quat_t &r ) { - quat_t q = { w, v }; - q = q*r; - w = q.w; - v = q.v; -} - -// Self divide -void quat_t::operator /= ( quat_t &r ) { - quat_t q = { w, v }; - q = q/r; - w = q.w; - v = q.v; -} - -//-- Global operators: - -// Vector multiplication - Result is quaternion -quat_t operator * ( vec3_t &v, vec3_t &r ) { - quat_t q = { -v.dot(r) , - v.cross(r) }; - return q; -} - -// Reverse order - Scalar product -quat_t operator * ( const float s, quat_t &r ) { - quat_t q = { r.w * s , - r.v * s }; - return q; -} - -//--------------- Important operations --------------- - -// Conjugate -quat_t quat_t::conj() { - quat_t q = { w , - -v }; - return q; -} - -// Inner product -float quat_t::inner() { - return w*w + v.dot(v); -} - -// Magnitude -float quat_t::mag() { - return sqrt( inner() ); -} - -// Normalize -quat_t quat_t::norm() { - quat_t q = { w, v }; - return q/mag(); -} - -//--------------- Rotation transform ----------------- - -// Transform as axis and angle -void quat_t::setRotation( vec3_t axis, float ang, const bool SMALL_ANG ) { - ang *= 0.5; - if( SMALL_ANG ) { - w = 1 - ang*ang; - v = ang * axis.norm(); - } else { - w = cos(ang); - v = sin(ang) * axis.norm(); - } -} - -// Transform as vector with magnitude of sin(angle) -void quat_t::setRotation( vec3_t u, const bool SMALL_ANG ) { - if( SMALL_ANG ) { - v = 0.5 * u; - w = 1 - 0.5*v.dot(v); - } else { - float mag = u.dot(u); - float sine = ( 1 - sqrt(1 - mag) )*0.5; - w = sqrt(1 - sine); - v = sqrt(sine/mag) * u; - } -} - -// Rotate vector -vec3_t quat_t::rotate( vec3_t r, const bool TO_GLOBAL ) { - float cross = 2*w; - if( TO_GLOBAL ) { - cross = -cross; - } - return ( w*w - v.dot(v) )*r + cross*v.cross(r) + ( 2*v.dot(r) )*v; -} - -//-- Axis vector projections: - -vec3_t quat_t::axisX( const bool TO_GLOBAL ) { - float w_vz = w*v.z; - float w_vy = w*v.y; - - if( TO_GLOBAL ) { - w_vz = -w_vz; - w_vy = -w_vy; - } - vec3_t u = { 2*( v.x*v.x + w*w ) - 1 , - 2*( v.x*v.y + w_vz ) , - 2*( v.x*v.z - w_vy ) }; - return u; -} - -vec3_t quat_t::axisY( const bool TO_GLOBAL ) { - float w_vz = w*v.z; - float w_vx = w*v.x; - - if( TO_GLOBAL ) { - w_vz = -w_vz; - w_vx = -w_vx; - } - vec3_t u = { 2*( v.y*v.x - w_vz ) , - 2*( v.y*v.y + w*w ) - 1 , - 2*( v.y*v.z + w_vx ) }; - return u; -} - -vec3_t quat_t::axisZ( const bool TO_GLOBAL ) { - float w_vy = w*v.y; - float w_vx = w*v.x; - - if( TO_GLOBAL ) { - w_vy = -w_vy; - w_vx = -w_vx; - } - vec3_t u = { 2*( v.z*v.x + w_vy ) , - 2*( v.z*v.y - w_vx ) , - 2*( v.z*v.z + w*w ) - 1 }; - return u; -} +#include "quaternion_type.h" + +//------------------- Constructors ------------------- + +// Default to zero vector +quat_t::quat_t() { + w = 1; + v = {0,0,0}; +} + +// Individual components +quat_t::quat_t( float _w, float _x, float _y, float _z ) { + w = _w; + v = { _x, _y, _z }; +} + +// Scalar and vector components +quat_t::quat_t( float _w, vec3_t _v ) { + w = _w; + v = _v; +} + +// Convert to vector +quat_t::quat_t( vec3_t _v ) { + w = 0; + v = _v; +} + +// Convert from array +quat_t::quat_t( float arr[] ) { + w = arr[0]; + v.x = arr[1]; + v.y = arr[2]; + v.z = arr[3]; +} + +//----------------- Basic Operations ------------------ + +//-- Addition and subtraction: + +// Addition +quat_t quat_t::operator + ( const quat_t &r ) { + quat_t q = { w + r.w , + v + r.v }; + return q; +} + +// Subtraction +quat_t quat_t::operator - ( const quat_t &r ) { + quat_t q = { w - r.w , + v - r.v }; + return q; +} +// Negation +quat_t quat_t::operator - ( void ) { + quat_t q = { -w , + -v }; + return q; +} + +// Increment +void quat_t::operator += ( const quat_t &r ) { + w += r.w; + v += r.v; +} +// Decrement +void quat_t::operator -= ( const quat_t &r ) { + w -= r.w; + v -= r.v; +} + +//-- Scalar product and division: + +// Scalar product +quat_t quat_t::operator * ( const float s ) { + quat_t q = { w * s , + v * s }; + return q; +} + +// Scalar division +quat_t quat_t::operator / ( const float s ) { + quat_t q = { w / s , + v / s }; + return q; +} + +// Self scalar multiply +void quat_t::operator *= ( const float s ) { + w *= s; + v *= s; +} + +// Self scalar divide +void quat_t::operator /= ( const float s ) { + w /= s; + v /= s; +} + +//--------------- Important operations --------------- + +// Conjugate +quat_t quat_t::conj() { + quat_t q = { w , + -v }; + return q; +} + +// Inner product +float quat_t::inner() { + return w*w + v.dot(v); +} + +// Magnitude +float quat_t::mag() { + return sqrt( inner() ); +} + +// Normalize +quat_t quat_t::norm() { + quat_t q = { w, v }; + return q/mag(); +} + +//----- Quaternion multiplication and division ------- + +// Multiplication +quat_t quat_t::operator * ( const quat_t &r ) { + quat_t q = { w*r.w - v.dot(r.v) , + w*r.v + v*r.w + v.cross(r.v) }; + return q; +} + +// Division +quat_t quat_t::operator / ( quat_t &r ) { + quat_t q = { w, v }; + return ( q*r.conj() )/r.inner(); +} + +// Self multiply +void quat_t::operator *= ( const quat_t &r ) { + quat_t q = { w, v }; + q = q*r; + w = q.w; + v = q.v; +} + +// Self divide +void quat_t::operator /= ( quat_t &r ) { + quat_t q = { w, v }; + q = q/r; + w = q.w; + v = q.v; +} + +//------------ Set as rotation transform ------------- + +// Axis and angle +void quat_t::setRotation( const bool SMALL_ANG, vec3_t axis, float ang ) { + ang *= 0.5; + if( SMALL_ANG ) { + w = 1 - ang*ang; + v = ang * axis.norm(); + } else { + w = cos(ang); + v = sin(ang) * axis.norm(); + } +} + +// Vector magniude is sine of angle +void quat_t::setRotation( const bool SMALL_ANG, vec3_t u ) { + if( SMALL_ANG ) { + v = 0.5 * u; + w = 1 - 0.5*v.dot(v); + } else { + float mag = u.dot(u); + float sine = ( 1 - sqrt(1 - mag) )*0.5; + w = sqrt(1 - sine); + v = sqrt(sine/mag) * u; + } +} + +//------------ Axis vector projections --------------- + +vec3_t quat_t::axisX( const bool INTO_ANG ) { + float w_vz = w*v.z; + float w_vy = w*v.y; + // + if( INTO_ANG ) { + w_vz = -w_vz; + w_vy = -w_vy; + } + vec3_t u = { 2*( v.x*v.x + w*w ) - 1 , + 2*( v.x*v.y + w_vz ) , + 2*( v.x*v.z - w_vy ) }; + return u; +} + +vec3_t quat_t::axisY( const bool INTO_ANG ) { + float w_vz = w*v.z; + float w_vx = w*v.x; + // + if( INTO_ANG ) { + w_vz = -w_vz; + w_vx = -w_vx; + } + vec3_t u = { 2*( v.y*v.x - w_vz ) , + 2*( v.y*v.y + w*w ) - 1 , + 2*( v.y*v.z + w_vx ) }; + return u; +} + +vec3_t quat_t::axisZ( const bool INTO_ANG ) { + float w_vy = w*v.y; + float w_vx = w*v.x; + // + if( INTO_ANG ) { + w_vy = -w_vy; + w_vx = -w_vx; + } + vec3_t u = { 2*( v.z*v.x + w_vy ) , + 2*( v.z*v.y - w_vx ) , + 2*( v.z*v.z + w*w ) - 1 }; + return u; +} + +//------------------ Vector rotation ------------------ + +// Rotate vector into angle +vec3_t quat_t::rotate( const bool INTO_ANG, vec3_t r ) { + float cross = 2*w; + if( INTO_ANG ) { + cross = -cross; + } + return ( w*w - v.dot(v) )*r + cross*v.cross(r) + ( 2*v.dot(r) )*v; +} + +//----------------- Global operators ----------------- + +// Vector multiplication - Result is quaternion +quat_t operator * ( vec3_t &v, vec3_t &r ) { + quat_t q = { -v.dot(r) , + v.cross(r) }; + return q; +} + +// Reverse order - Scalar product +quat_t operator * ( const float s, quat_t &r ) { + quat_t q = { r.w * s , + r.v * s }; + return q; +} \ No newline at end of file diff --git a/src/quaternion_type.h b/src/quaternion_type.h index cf04dc9..8e58c99 100644 --- a/src/quaternion_type.h +++ b/src/quaternion_type.h @@ -3,73 +3,67 @@ #ifndef quaternion_type_h #define quaternion_type_h - -// input labels -#define LOCAL_FRAME true -#define GLOBAL_FRAME false - -#define SMALL_ANGLE true -#define LARGE_ANGLE false //------- 4D Quaternion -------- struct quat_t { - // Components - float w; // scalar - vec3_t v; // vector + // Components: + float w; + vec3_t v; - // 0. Constructors: + // Constructors: quat_t(); quat_t( float, float, float, float ); quat_t( float, vec3_t ); quat_t( vec3_t ); quat_t( float [] ); - - // 1. Basic Operations: - // Addition and subtraction: + + //- Basic Operations: + + // Addition and subtraction: quat_t operator + ( const quat_t & ); quat_t operator - ( const quat_t & ); quat_t operator - ( void ); void operator += ( const quat_t & ); void operator -= ( const quat_t & ); - // Scalar product and division: + // Scalar product and division: quat_t operator * ( const float ); quat_t operator / ( const float ); void operator *= ( const float ); void operator /= ( const float ); - // Quaternion multiplication and division: + //- Important operations: + quat_t conj(); + float inner(); + float mag(); + quat_t norm(); + + //- Quaternion multiplication and division: quat_t operator * ( const quat_t & ); quat_t operator / ( quat_t & ); void operator *= ( const quat_t & ); void operator /= ( quat_t & ); - // 3. Important operations: - quat_t conj(); - quat_t norm(); - float inner(); - float mag(); + //- Set as rotation transform: - // 4. Rotation transform: - // axis and angle - void setRotation( vec3_t, float, const bool ); - void setRotation( vec3_t, const bool ); + // Axis and angle + void setRotation( const bool, vec3_t, float ); + void setRotation( const bool, vec3_t ); - // vector rotation - vec3_t rotate( vec3_t, const bool ); + // Vector rotation: + vec3_t rotate( const bool, vec3_t ); - // axis projections + // Axis vector projections: vec3_t axisX( const bool ); vec3_t axisY( const bool ); vec3_t axisZ( const bool ); }; -// 1B. Global operators: - // vector multiplication +// Vector multiplication - Result is quaternion quat_t operator * ( vec3_t &, vec3_t & ); - // reverse order scalar product +// Reverse order - Scalar product quat_t operator * ( const float, quat_t & ); #endif diff --git a/src/vector_type.cpp b/src/vector_type.cpp index 1dd56ca..9193e16 100644 --- a/src/vector_type.cpp +++ b/src/vector_type.cpp @@ -1,134 +1,141 @@ -#include "vector_type.h" - -//------------------- Constructors ------------------- - -vec3_t::vec3_t() {} - -// Specific components -vec3_t::vec3_t( float _x, float _y, float _z ) { - x = _x; - y = _y; - z = _z; -} - -// 2D vector: -vec3_t::vec3_t( float _x, float _y ) { - x = _x; - y = _y; - z = 0; -} - -// Convert from array -vec3_t::vec3_t( float arr[] ) { - x = arr[0]; - y = arr[1]; - z = arr[2]; -} - -//---------------- Basic operations ------------------ - -//-- Addition and subtration: - -// Addition -vec3_t vec3_t::operator + ( const vec3_t &r ) { - vec3_t v = { x + r.x , - y + r.y , - z + r.z }; - return v; -} - -// Subtraction -vec3_t vec3_t::operator - ( const vec3_t &r ) { - vec3_t v = { x - r.x , - y - r.y , - z - r.z }; - return v; -} - -// Negation -vec3_t vec3_t::operator - ( void ) { - vec3_t v = { -x , - -y , - -z }; - return v; -} - -// Increment -void vec3_t::operator += ( const vec3_t &r ) { - x += r.x; - y += r.y; - z += r.z; -} - -// Decrement -void vec3_t::operator -= ( const vec3_t &r ) { - x -= r.x; - y -= r.y; - z -= r.z; -} - -//-------- Scalar multiplication and division -------- - -// Scalar product -vec3_t vec3_t::operator * ( const float s ) { - vec3_t v = { x * s , - y * s , - z * s }; - return v; -} - -// Scalar division -vec3_t vec3_t::operator / ( const float s ) { - vec3_t v = { x / s , - y / s , - z / s }; - return v; -} - -// Self multiply -void vec3_t::operator *= ( const float s ) { - x *= s; - y *= s; - z *= s; -} - -// Self divide -void vec3_t::operator /= ( const float s ) { - x /= s; - y /= s; - z /= s; -} - -// Reverse order - Scalar product --- [Global operator] -vec3_t operator * ( const float s, const vec3_t &r ) { - vec3_t v = { r.x * s , - r.y * s , - r.z * s }; - return v; -} - -//--------------- Important operations --------------- - -// Dot product -float vec3_t::dot( const vec3_t r ) { - return x*r.x + y*r.y + z*r.z; -} - -// Cross product -vec3_t vec3_t::cross( const vec3_t r ) { - vec3_t v = { y*r.z - z*r.y , - -x*r.z + z*r.x , - x*r.y - y*r.x }; - return v; -} - -// Magnitude -float vec3_t::mag() { - return sqrt( x*x + y*y + z*z ); -} - -// Normalize -vec3_t vec3_t::norm() { - vec3_t v = { x, y, z }; - return v/mag(); -} +#include "vector_type.h" + +//------------------- Constructors ------------------- + +// Default to zero +vec3_t::vec3_t() { + x = 0; + y = 0; + z = 0; +} + +// Specific components +vec3_t::vec3_t( float _x, float _y, float _z ) { + x = _x; + y = _y; + z = _z; +} + +// 2D vector: +vec3_t::vec3_t( float _x, float _y ) { + x = _x; + y = _y; + z = 0; +} + +// Convert from array +vec3_t::vec3_t( float arr[] ) { + x = arr[0]; + y = arr[1]; + z = arr[2]; +} + +//---------------- Basic operations ------------------ + +//-- Addition and subtration: + +// Addition +vec3_t vec3_t::operator + ( const vec3_t &r ) { + vec3_t v = { x + r.x , + y + r.y , + z + r.z }; + return v; +} + +// Subtraction +vec3_t vec3_t::operator - ( const vec3_t &r ) { + vec3_t v = { x - r.x , + y - r.y , + z - r.z }; + return v; +} + +// Negation +vec3_t vec3_t::operator - ( void ) { + vec3_t v = { -x , + -y , + -z }; + return v; +} + +// Increment +void vec3_t::operator += ( const vec3_t &r ) { + x += r.x; + y += r.y; + z += r.z; +} + +// Decrement +void vec3_t::operator -= ( const vec3_t &r ) { + x -= r.x; + y -= r.y; + z -= r.z; +} + +//-------- Scalar multiplication and division -------- + +// Scalar product +vec3_t vec3_t::operator * ( const float s ) { + vec3_t v = { x * s , + y * s , + z * s }; + return v; +} + +// Scalar division +vec3_t vec3_t::operator / ( const float s ) { + vec3_t v = { x / s , + y / s , + z / s }; + return v; +} + +// Self multiply +void vec3_t::operator *= ( const float s ) { + x *= s; + y *= s; + z *= s; +} + +// Self divide +void vec3_t::operator /= ( const float s ) { + x /= s; + y /= s; + z /= s; +} + +//--------------- Important operations --------------- + +// Dot product +float vec3_t::dot( const vec3_t r ) { + return x*r.x + y*r.y + z*r.z; +} + +// Cross product +vec3_t vec3_t::cross( const vec3_t r ) { + vec3_t v = { y*r.z - z*r.y , + -x*r.z + z*r.x , + x*r.y - y*r.x }; + return v; +} + +// Magnitude +float vec3_t::mag() { + return sqrt( x*x + y*y + z*z ); +} + +// Normalize +vec3_t vec3_t::norm() { + vec3_t v = { x, y, z }; + return v/mag(); +} + +//----------------- Global operators ----------------- + +// Reverse order - Scalar product +vec3_t operator * ( const float s, const vec3_t &r ) { + vec3_t v = { r.x * s , + r.y * s , + r.z * s }; + return v; +} \ No newline at end of file diff --git a/src/vector_type.h b/src/vector_type.h index f9f06eb..77dd469 100644 --- a/src/vector_type.h +++ b/src/vector_type.h @@ -11,34 +11,35 @@ struct vec3_t { float y; float z; - // 0. Constructors: + //- Constructors: vec3_t(); vec3_t( float, float, float ); vec3_t( float, float ); vec3_t( float [] ); - // 1. Basic operations: - // Addition and subtration: + //- Basic operations: + + // Addition and subtration: vec3_t operator + ( const vec3_t & ); vec3_t operator - ( const vec3_t & ); vec3_t operator - ( void ); void operator += ( const vec3_t & ); void operator -= ( const vec3_t & ); - // Scalar multiplication and division: + // Scalar multiplication and division: vec3_t operator * ( const float ); vec3_t operator / ( const float ); void operator *= ( const float ); void operator /= ( const float ); - // Important operations: + // Important operations: float dot( const vec3_t ); vec3_t cross( const vec3_t ); float mag(); vec3_t norm(); }; -// 1B. Reverse order - Scalar product +// Reverse order - Scalar product vec3_t operator * ( const float, const vec3_t & ); #endif