Line data Source code
1 : #pragma once
2 : #include "math3d/vec3.hpp"
3 :
4 : namespace m3d
5 : {
6 : // Forward declarations
7 : struct quat;
8 : inline quat normalize(const quat &q);
9 :
10 : struct quat
11 : {
12 : // Layout: w is real part, v=(x,y,z) is imaginary part
13 : scalar w, x, y, z;
14 :
15 55288 : quat() : w(1), x(0), y(0), z(0) {} // Identity
16 88767 : quat(scalar _w, scalar _x, scalar _y, scalar _z) : w(_w), x(_x), y(_y), z(_z) {}
17 :
18 : // Construct from axis-angle
19 6 : static quat from_axis_angle(const vec3 &axis, scalar angle)
20 : {
21 6 : scalar half_angle = angle * 0.5;
22 6 : scalar s = m3d::sin(half_angle);
23 6 : vec3 n = normalize(axis);
24 6 : return {m3d::cos(half_angle), n.x * s, n.y * s, n.z * s};
25 : }
26 :
27 18 : static quat from_rpy(scalar roll, scalar pitch, scalar yaw)
28 : {
29 18 : scalar cr = m3d::cos(roll * 0.5);
30 18 : scalar sr = m3d::sin(roll * 0.5);
31 18 : scalar cp = m3d::cos(pitch * 0.5);
32 18 : scalar sp = m3d::sin(pitch * 0.5);
33 18 : scalar cy = m3d::cos(yaw * 0.5);
34 18 : scalar sy = m3d::sin(yaw * 0.5);
35 :
36 : return {
37 18 : cr * cp * cy + sr * sp * sy, // w
38 18 : sr * cp * cy - cr * sp * sy, // x
39 18 : cr * sp * cy + sr * cp * sy, // y
40 18 : cr * cp * sy - sr * sp * cy // z
41 18 : };
42 : }
43 :
44 : // Quaternion Multiplication (Hamilton Product)
45 54744 : quat operator*(const quat &r) const
46 : {
47 54744 : quat res;
48 54744 : res.w = w * r.w - x * r.x - y * r.y - z * r.z;
49 54744 : res.x = w * r.x + x * r.w + y * r.z - z * r.y;
50 54744 : res.y = w * r.y + y * r.w + z * r.x - x * r.z;
51 54744 : res.z = w * r.z + z * r.w + x * r.y - y * r.x;
52 54744 : return res; //normalize(res);
53 : }
54 :
55 : // Scaling
56 18716 : quat operator*(scalar s) const { return {w * s, x * s, y * s, z * s}; }
57 :
58 : // Addition (useful for integration)
59 18715 : quat operator+(const quat &r) const { return {w + r.w, x + r.x, y + r.y, z + r.z}; }
60 :
61 2 : quat &operator+=(const quat &rhs)
62 : {
63 2 : w += rhs.w;
64 2 : x += rhs.x;
65 2 : y += rhs.y;
66 2 : z += rhs.z;
67 2 : return *this;
68 : }
69 :
70 1 : friend std::ostream &operator<<(std::ostream &os, const quat &q)
71 : {
72 1 : return os << "quat(w:" << q.w << ", x:" << q.x << ", y:" << q.y << ", z:" << q.z << ")";
73 : }
74 :
75 : // Inside struct quat
76 9 : bool operator==(const quat &other) const
77 : {
78 9 : return w == other.w && x == other.x && y == other.y && z == other.z;
79 : }
80 :
81 : bool is_approx(const quat &other, scalar precision = EPSILON) const
82 : {
83 : return m3d::abs(w - other.w) < precision &&
84 : m3d::abs(x - other.x) < precision &&
85 : m3d::abs(y - other.y) < precision &&
86 : m3d::abs(z - other.z) < precision;
87 : }
88 : };
89 :
90 18715 : inline quat normalize(const quat &q)
91 : {
92 18715 : scalar mag_sq = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
93 :
94 : // Avoid the sqrt call
95 18715 : if (m3d::abs(1.0 - mag_sq) < 2.107342e-08)
96 : { // Typical float precision threshold
97 18703 : return q * (2.0 / (1.0 + mag_sq));
98 : }
99 :
100 12 : if (mag_sq < EPSILON)
101 0 : return {1, 0, 0, 0}; // Avoid zero division
102 :
103 12 : return q * (1.0 / m3d::sqrt(mag_sq));
104 : }
105 :
106 32000 : inline quat conjugate(const quat &q)
107 : {
108 32000 : return {q.w, -q.x, -q.y, -q.z};
109 : }
110 :
111 : // Rotate vector v by quaternion q ( q * v * q' )
112 : // Optimized formula: v + 2 * cross(q.xyz, cross(q.xyz, v) + q.w * v)
113 67649 : inline vec3 rotate(const vec3 &v, const quat &q)
114 : {
115 67649 : vec3 u(q.x, q.y, q.z);
116 67649 : scalar s = q.w;
117 :
118 67649 : vec3 uv = cross(u, v);
119 67649 : vec3 uuv = cross(u, uv);
120 :
121 67649 : return v + ((uv * s) + uuv) * 2.0;
122 : }
123 :
124 67646 : inline vec3 rotate(const quat &q, const vec3 &v)
125 : {
126 67646 : return rotate(v, q);
127 : }
128 :
129 : inline vec3 to_rpy(const quat &q)
130 : {
131 : quat n = normalize(q);
132 :
133 : // roll (x-axis rotation)
134 : scalar sinr_cosp = 2.0 * (n.w * n.x + n.y * n.z);
135 : scalar cosr_cosp = 1.0 - 2.0 * (n.x * n.x + n.y * n.y);
136 : scalar roll = m3d::atan2(sinr_cosp, cosr_cosp);
137 :
138 : // pitch (y-axis rotation)
139 : scalar sinp = 2.0 * (n.w * n.y - n.z * n.x);
140 :
141 : scalar pitch;
142 : if (m3d::abs(sinp) >= 1.0)
143 : pitch = std::copysign(M_PI / 2.0, sinp); // clamp at 90°
144 : else
145 : pitch = std::asin(sinp);
146 :
147 : // yaw (z-axis rotation)
148 : scalar siny_cosp = 2.0 * (n.w * n.z + n.x * n.y);
149 : scalar cosy_cosp = 1.0 - 2.0 * (n.y * n.y + n.z * n.z);
150 : scalar yaw = m3d::atan2(siny_cosp, cosy_cosp);
151 :
152 : return {roll, pitch, yaw};
153 : }
154 :
155 : } // namespace m3d
|
