├── Coord.cs ├── LICENSE ├── README.md ├── coord.hpp ├── coord2.hpp ├── lorentz_coord.hpp └── quaternion.hpp /Coord.cs: -------------------------------------------------------------------------------- 1 | /// 2 | /// *************************************************************************************** 3 | /// Represents a three-dimensional coordinate system in Unity. 4 | /// *************************************************************************************** 5 | /// 6 | [System.Serializable] 7 | public struct UCoord3 8 | { 9 | public static readonly UCoord3 zero = new UCoord3(Vector3.zero, Vector3.zero, Vector3.zero); 10 | public static readonly UCoord3 one = new UCoord3(Vector3.right, Vector3.up, Vector3.forward); 11 | 12 | public Vector3 right; 13 | public Vector3 up; 14 | public Vector3 forward; 15 | 16 | /// 17 | /// Creates a new UCoord3 instance with the specified vectors as the axes. 18 | /// 19 | /// The right-axis vector. 20 | /// The up-axis vector. 21 | /// The forward-axis vector. 22 | public UCoord3(Vector3 right, Vector3 up, Vector3 forward) 23 | { 24 | this.right = right; 25 | this.up = up; 26 | this.forward = forward; 27 | } 28 | 29 | /// 30 | /// Creates a new UCoord3 instance with the specified vectors as the axes and calculates the forward-axis vector. 31 | /// 32 | /// The right-axis vector. 33 | /// The up-axis vector. 34 | public UCoord3(Vector3 right, Vector3 up) 35 | { 36 | this.right = right; 37 | this.up = up; 38 | this.forward = Vector3.Cross(right, up); 39 | } 40 | 41 | /// 42 | /// Creates a new UCoord3 instance with the specified rotation angle around the specified axis. 43 | /// 44 | /// The rotation angle in degrees. 45 | /// The rotation axis vector. 46 | public UCoord3(float angle, Vector3 axis) 47 | { 48 | Quaternion q = Quaternion.AngleAxis(angle, axis); 49 | 50 | this.right = q * Vector3.right; 51 | this.up = q * Vector3.up; 52 | this.forward = q * Vector3.forward; 53 | } 54 | public UCoord3(Quaternion q) 55 | { 56 | this.right = q * Vector3.right; 57 | this.up = q * Vector3.up; 58 | this.forward = q * Vector3.forward; 59 | } 60 | /// 61 | /// Sets the UCoord3 instance based on the rotation between two vectors. 62 | /// 63 | /// The first vector. 64 | /// The second vector. 65 | public void FromVecsR(Vector3 v1, Vector3 v2) 66 | { 67 | Quaternion q = Quaternion.FromToRotation(v1, v2); 68 | this.right = q * Vector3.right; 69 | this.up = q * Vector3.up; 70 | this.forward = q * Vector3.forward; 71 | } 72 | 73 | /// 74 | /// Converts the UCoord3 instance to a Quaternion. 75 | /// 76 | /// The Quaternion representation of the UCoord3 instance. 77 | public Quaternion ToQuaternion() 78 | { 79 | Vector3 eulers = ToEulerAngles(); 80 | Quaternion q = Quaternion.Euler(eulers); 81 | return q; 82 | } 83 | 84 | /// 85 | /// Checks if the current UCoord3 instance has the same direction as the specified UCoord3. 86 | /// 87 | /// The UCoord3 to compare with. 88 | /// True if the directions are the same, false otherwise. 89 | public bool SameDirections(UCoord3 c) 90 | { 91 | return this.right == c.right && this.up == c.up && this.forward == c.forward; 92 | } 93 | 94 | /// 95 | /// Transforms the specified vector using the UCoord3 instance. 96 | /// 97 | /// The vector to transform. 98 | /// The UCoord3 instance. 99 | /// The transformed vector. 100 | public static Vector3 operator *(Vector3 p, UCoord3 c) 101 | { 102 | return c.right * p.x + c.up * p.y + c.forward * p.z; 103 | } 104 | 105 | /// 106 | /// Multiplies two UCoord3 instances together. 107 | /// 108 | /// The first UCoord3. 109 | /// The second UCoord3. 110 | /// The result of the multiplication. 111 | public static UCoord3 operator *(UCoord3 c1, UCoord3 c2) 112 | { 113 | UCoord3 rc = new UCoord3(); 114 | rc.right = new Vector3(c1.right.x * c2.right.x + c1.right.y * c2.up.x + c1.right.z * c2.forward.x, 115 | c1.right.x * c2.right.y + c1.right.y * c2.up.y + c1.right.z * c2.forward.y, 116 | c1.right.x * c2.right.z + c1.right.y * c2.up.z + c1.right.z * c2.forward.z); 117 | rc.up = new Vector3(c1.up.x * c2.right.x + c1.up.y * c2.up.x + c1.up.z * c2.forward.x, 118 | c1.up.x * c2.right.y + c1.up.y * c2.up.y + c1.up.z * c2.forward.y, 119 | c1.up.x * c2.right.z + c1.up.y * c2.up.z + c1.up.z * c2.forward.z); 120 | rc.forward = new Vector3(c1.forward.x * c2.right.x + c1.forward.y * c2.up.x + c1.forward.z * c2.forward.x, 121 | c1.forward.x * c2.right.y + c1.forward.y * c2.up.y + c1.forward.z * c2.forward.y, 122 | c1.forward.x * c2.right.z + c1.forward.y * c2.up.z + c1.forward.z * c2.forward.z); 123 | return rc; 124 | } 125 | 126 | /// 127 | /// Multiplies a Quaternion and a UCoord3 together. 128 | /// 129 | /// The Quaternion. 130 | /// The UCoord3. 131 | /// The result of the multiplication. 132 | public static Quaternion operator *(Quaternion q, UCoord3 c) 133 | { 134 | Quaternion q1 = c.ToQuaternion(); 135 | return q * q1; 136 | } 137 | 138 | /// 139 | /// Multiplies a UCoord3 and a Quaternion together. 140 | /// 141 | /// The UCoord3. 142 | /// The Quaternion. 143 | /// The result of the multiplication. 144 | public static UCoord3 operator *(UCoord3 c, Quaternion q) 145 | { 146 | UCoord3 rc = new UCoord3(); 147 | rc.right = q * c.right; 148 | rc.up = q * c.up; 149 | rc.forward = q * c.forward; 150 | return rc; 151 | } 152 | 153 | /// 154 | /// Divides two UCoord3 instances. 155 | /// 156 | /// The numerator UCoord3. 157 | /// The denominator UCoord3. 158 | /// The result of the division. 159 | public static UCoord3 operator /(UCoord3 c1, UCoord3 c2) 160 | { 161 | UCoord3 rc = new UCoord3(); 162 | rc.right = new Vector3(c1.right.x / c2.right.x + c1.right.y / c2.up.x + c1.right.z / c2.forward.x, 163 | c1.right.x / c2.right.y + c1.right.y / c2.up.y + c1.right.z / c2.forward.y, 164 | c1.right.x / c2.right.z + c1.right.y / c2.up.z + c1.right.z / c2.forward.z); 165 | rc.up = new Vector3(c1.up.x / c2.right.x + c1.up.y / c2.up.x + c1.up.z / c2.forward.x, 166 | c1.up.x / c2.right.y + c1.up.y / c2.up.y + c1.up.z / c2.forward.y, 167 | c1.up.x / c2.right.z + c1.up.y / c2.up.z + c1.up.z / c2.forward.z); 168 | rc.forward = new Vector3(c1.forward.x / c2.right.x + c1.forward.y / c2.up.x + c1.forward.z / c2.forward.x, 169 | c1.forward.x / c2.right.y + c1.forward.y / c2.up.y + c1.forward.z / c2.forward.y, 170 | c1.forward.x / c2.right.z + c1.forward.y / c2.up.z + c1.forward.z / c2.forward.z); 171 | return rc; 172 | } 173 | 174 | /// 175 | /// Divides a vector by a UCoord3. 176 | /// 177 | /// The vector to divide. 178 | /// The UCoord3. 179 | /// The result of the division. 180 | public static Vector3 operator /(Vector3 v, UCoord3 c) 181 | { 182 | return new Vector3(Vector3.Dot(v, c.right), Vector3.Dot(v, c.up), Vector3.Dot(v, c.forward)); 183 | } 184 | 185 | /// 186 | /// Calculates the reverse of the UCoord3 instance. 187 | /// 188 | /// The reverse of the UCoord3 instance. 189 | public UCoord3 Reversed() 190 | { 191 | return one / this; 192 | } 193 | 194 | /// 195 | /// Converts the UCoord3 instance to Euler angles. 196 | /// 197 | /// The Euler angles representation of the UCoord3 instance. 198 | public Vector3 ToEulerAngles() 199 | { 200 | Quaternion q = Quaternion.LookRotation(forward, up); 201 | Vector3 eulerAngles = q.eulerAngles; 202 | float pitch = eulerAngles.x; 203 | float yaw = eulerAngles.y; 204 | float roll = eulerAngles.z; 205 | return new Vector3(pitch, yaw, roll); 206 | } 207 | } 208 | 209 | /// 210 | /// *************************************************************************************** 211 | /// Represents a three-dimensional coordinate system with scaling and translation in Unity. 212 | /// *************************************************************************************** 213 | /// 214 | [System.Serializable] 215 | public struct Coord3 216 | { 217 | public static readonly Coord3 zero = new Coord3(UCoord3.one, Vector3.one, Vector3.zero); 218 | public static readonly Coord3 one = new Coord3(UCoord3.one, Vector3.one, Vector3.zero); 219 | 220 | public Vector3 origin; 221 | public Vector3 scale; 222 | public UCoord3 uCoord; 223 | 224 | /// 225 | /// Creates a new Coord3 instance with the specified UCoord3, scale, and origin. 226 | /// 227 | /// The UCoord3 instance. 228 | /// The scale vector. 229 | /// The origin vector. 230 | public Coord3(UCoord3 uCoord, Vector3 scale, Vector3 origin) 231 | { 232 | this.uCoord = uCoord; 233 | this.scale = scale; 234 | this.origin = origin; 235 | } 236 | /// 237 | /// Creates a new Coord3 instance with the specified UCoord3 and origin. 238 | /// 239 | /// The UCoord3 instance. 240 | /// The origin vector. 241 | public Coord3(UCoord3 uCoord, Vector3 origin) 242 | { 243 | this.uCoord = uCoord; 244 | this.scale = Vector3.one; 245 | this.origin = origin; 246 | } 247 | 248 | /// 249 | /// Creates a new Coord3 instance with the specified UCoord3. 250 | /// 251 | /// The UCoord3 instance. 252 | public Coord3(UCoord3 uCoord) 253 | { 254 | this.uCoord = uCoord; 255 | this.scale = Vector3.one; 256 | this.origin = Vector3.zero; 257 | } 258 | 259 | /// 260 | /// Creates a new Coord3 instance with the specified origin. 261 | /// 262 | /// The origin vector. 263 | public Coord3(Vector3 origin) 264 | { 265 | this.uCoord = UCoord3.one; 266 | this.scale = Vector3.one; 267 | this.origin = origin; 268 | } 269 | 270 | /// 271 | /// Creates a new Coord3 instance with the specified x-axis, y-axis, and z-axis vectors. 272 | /// 273 | /// The x-axis vector. 274 | /// The y-axis vector. 275 | /// The z-axis vector. 276 | public Coord3(Vector3 ux, Vector3 uy, Vector3 uz) 277 | { 278 | this.uCoord = new UCoord3(ux, uy, uz); 279 | this.scale = Vector3.one; 280 | this.origin = Vector3.zero; 281 | } 282 | 283 | /// 284 | /// Creates a new Coord3 instance with the specified x-axis and y-axis vectors. 285 | /// 286 | /// The x-axis vector. 287 | /// The y-axis vector. 288 | public Coord3(Vector3 ux, Vector3 uy) 289 | { 290 | this.uCoord = new UCoord3(ux, uy); 291 | this.scale = Vector3.one; 292 | this.origin = Vector3.zero; 293 | } 294 | 295 | /// 296 | /// Creates a new Coord3 instance with the specified x-axis, y-axis, z-axis, and origin vectors. 297 | /// 298 | /// The x-axis vector. 299 | /// The y-axis vector. 300 | /// The z-axis vector. 301 | /// The origin vector. 302 | public Coord3(Vector3 ux, Vector3 uy, Vector3 uz, Vector3 origin) 303 | { 304 | this.uCoord = new UCoord3(ux, uy, uz); 305 | this.scale = Vector3.one; 306 | this.origin = origin; 307 | } 308 | 309 | /// 310 | /// Creates a new Coord3 instance with the specified x-axis, y-axis, z-axis, scale, and origin vectors. 311 | /// 312 | /// The x-axis vector. 313 | /// The y-axis vector. 314 | /// The z-axis vector. 315 | /// The scale vector. 316 | /// The origin vector. 317 | public Coord3(Vector3 ux, Vector3 uy, Vector3 uz, Vector3 scale, Vector3 origin) 318 | { 319 | this.uCoord = new UCoord3(ux, uy, uz); 320 | this.scale = scale; 321 | this.origin = origin; 322 | } 323 | 324 | /// 325 | /// Transforms the specified vector using the Coord3 instance. 326 | /// 327 | /// The vector to transform. 328 | /// The Coord3 instance. 329 | /// The transformed vector. 330 | public static Vector3 operator *(Vector3 p, Coord3 c) 331 | { 332 | return p.x * c.uCoord.right + p.y * c.uCoord.up + p.z * c.uCoord.forward + c.origin; 333 | } 334 | 335 | /// 336 | /// Multiplies two Coord3 instances together. 337 | /// 338 | /// The first Coord3. 339 | /// The second Coord3. 340 | /// The result of the multiplication. 341 | public static Coord3 operator *(Coord3 c1, Coord3 c2) 342 | { 343 | Coord3 rc = new Coord3(); 344 | rc.uCoord = c1.uCoord * c2.uCoord; 345 | rc.scale = new Vector3(c1.scale.x * c2.scale.x, c1.scale.y * c2.scale.y, c1.scale.z * c2.scale.z); 346 | rc.origin = c2.origin + c1.origin.x * c2.scale.x * c2.uCoord.right + c1.origin.y * c2.scale.y * c2.uCoord.up + c1.origin.z * c2.scale.z * c2.uCoord.forward; 347 | return rc; 348 | } 349 | 350 | /// 351 | /// Multiplies a Quaternion and a Coord3 together. 352 | /// 353 | /// The Quaternion. 354 | /// The Coord3. 355 | /// The result of the multiplication. 356 | public static Coord3 operator *(Quaternion q, Coord3 c) 357 | { 358 | Coord3 rc = c; 359 | rc.uCoord = rc.uCoord * q; 360 | rc.origin = q * rc.origin; 361 | return rc; 362 | } 363 | 364 | /// 365 | /// Multiplies a Coord3 and a Quaternion together. 366 | /// 367 | /// The Coord3. 368 | /// The Quaternion. 369 | /// The result of the multiplication. 370 | public static Coord3 operator *(Coord3 c, Quaternion q) 371 | { 372 | return new Coord3(c.uCoord * q); 373 | } 374 | 375 | /// 376 | /// Divides two Coord3 instances. 377 | /// 378 | /// The numerator Coord3. 379 | /// The denominator Coord3. 380 | /// The result of the division. 381 | public static Coord3 operator /(Coord3 c1, Coord3 c2) 382 | { 383 | Coord3 rc = new Coord3(); 384 | rc.uCoord = c1.uCoord / c2.uCoord; 385 | rc.scale = new Vector3(c1.scale.x / c2.scale.x, c1.scale.y / c2.scale.y, c1.scale.z / c2.scale.z); 386 | rc.origin = c1.origin / c2; 387 | return rc; 388 | } 389 | 390 | /// 391 | /// Divides a Coord3 by a Quaternion. 392 | /// 393 | /// The Coord3. 394 | /// The Quaternion. 395 | /// The result of the division. 396 | public static Coord3 operator /(Coord3 c, Quaternion q) 397 | { 398 | return c * Quaternion.Inverse(q); 399 | } 400 | 401 | /// 402 | /// Divides a vector by a Coord3. 403 | /// 404 | /// The vector to divide. 405 | /// The Coord3. 406 | /// The result of the division. 407 | public static Vector3 operator /(Vector3 p, Coord3 c) 408 | { 409 | Vector3 v = p - c.origin; 410 | return new Vector3(Vector3.Dot(v, c.uCoord.right) / c.scale.x, Vector3.Dot(v, c.uCoord.up) / c.scale.y, Vector3.Dot(v, c.uCoord.forward) / c.scale.z); 411 | } 412 | 413 | /// 414 | /// Calculates the reverse of the Coord3 instance. 415 | /// 416 | /// The reverse of the Coord3 instance. 417 | public Coord3 Reversed() 418 | { 419 | return one / this; 420 | } 421 | } 422 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 panguojun 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![image](https://github.com/user-attachments/assets/066f8e50-1146-4ebb-b319-cabc8b3f7add) 2 | 3 | # The Coordinate System (Coord) Framework 4 | 5 | ## History of Coordinate Systems 6 | The concept of coordinate systems traces back to René Descartes, who sought to use geometry to describe celestial motion. However, his methods lacked the precision required for exact calculations. Long before Descartes, early civilizations already had notions of coordinate-like references—particularly the idea of a "world center." 7 | 8 | During the Hellenistic period, Ptolemaic cosmology placed Earth at the center of the universe, while Copernicus later shifted this central reference to the Sun. The key difference between these models was not just the choice of origin but the mathematical framework they enabled. By repositioning the center at the Sun, scientists recognized the need for a dynamic, motion-based mathematical-physical system. This realization paved the way for calculus, equations of motion, and Newton's laws of inertia—cornerstones of modern science. 9 | 10 | Thus, the choice of coordinate system profoundly influences both worldview and computational paradigms. Einstein’s relativity theory, for instance, can be viewed as a consequence of extending coordinates from flat Euclidean space to curved manifolds. Moreover, it appears that all precisely calculable problems ultimately reduce to coordinate transformations. Approximate methods—such as statistical approaches in quantum mechanics and thermodynamics—remain necessary where exact solutions are intractable (though modern techniques like Density Functional Theory (DFT) have achieved notable, if still imperfect, success). 11 | 12 | ## Mathematical Foundation 13 | 14 | The Coordinate System (or Frame), referred to here as the Coord object, is a mathematical construct rooted in group theory that defines a coordinate system in three-dimensional space. In physics, such a structure is commonly known as a reference frame, while in differential geometry, it is often called a frame field or moving frame, borrowing terminology from classical mechanics. 15 | 16 | From a group-theoretic perspective, a coordinate system (or its simplified form, a coordinate) can be treated as an algebraic object capable of participating in group operations. The Coord object unifies these concepts, allowing both coordinate systems and individual coordinates to serve as elements in algebraic operations, such as multiplication and division. 17 | 18 | By extending coordinate systems with arithmetic operations (addition, subtraction, multiplication, and division), the Coord object enables direct differential calculus, eliminating the need for cumbersome exterior calculus formulations. This approach provides an intuitive geometric interpretation of operations like vector division, which traditionally require complex tensor algebra. Moreover, it simplifies advanced differential geometry concepts such as connections (affine or Levi-Civita) and curvature tensors, offering a unified and geometrically intuitive framework for computations involving coordinate systems. 19 | 20 | ## Applications 21 | World ↔ Local Coordinate Transformations: Seamlessly convert between global and local reference frames. 22 | 23 | Multi-Node Hierarchies: Efficiently manage transformations in complex systems (e.g., robotics, computer graphics). 24 | 25 | Differential Geometry & Physics: Streamline computations involving curvature, parallel transport, and dynamic reference frames. 26 | 27 | The Coord object thus serves as a powerful abstraction, bridging algebraic operations, differential calculus, and geometric intuition in a computationally elegant manner. 28 | 29 | ## Structure of the Coordinate System 30 | 31 | In C++, a coordinate system in three-dimensional space is defined by an origin, three directional axes, and three scaling components as follows: 32 | 33 | ``` 34 | struct coord { 35 | vec3 ux, uy, uz; // Three basis vectors 36 | vec3 s; // Scaling 37 | vec3 o; // Origin 38 | }; 39 | ``` 40 | 41 | ## Constructing a Coordinate System 42 | 43 | A coordinate system can be constructed using three axes or Euler angles as follows: 44 | 45 | ``` 46 | coord C1(vec3 o); 47 | coord C2(vec3 ux, vec3 uy, vec3 uz); 48 | coord C3(vec3 o, vec3 s, quat q); 49 | ``` 50 | 51 | ## Multiplication and Division Operations 52 | 53 | Multiplication and division operations are provided to transform vectors from one coordinate system to another and to project vectors from a parent coordinate system to a local one. For example, to transform a vector V1 from a local coordinate system C1 to a parent coordinate system C0, we can use the following operation: 54 | 55 | ``` 56 | V0 = V1 * C1 57 | ``` 58 | 59 | To project a vector V0 from a parent coordinate system C0 to a local coordinate system C1, we can use the following operation: 60 | 61 | ``` 62 | V1 = V0 / C1 63 | ``` 64 | 65 | ## Common Scenarios 66 | 67 | Coord can be applied in various scenarios, such as converting between world and local coordinate systems and using it in multi-node hierarchies. Here are some examples: 68 | 69 | 1. Convert a vector Vw from a world coordinate system to a local coordinate system C: 70 | 71 | ``` 72 | VL = Vw / C 73 | Vw = VL * C 74 | ``` 75 | 76 | 2. Convert between world and local coordinate systems: 77 | 78 | ``` 79 | C = C3 * C2 * C1 80 | Vw = VL * C 81 | VL = Vw / C 82 | ``` 83 | 84 | 3. Use in multi-node hierarchies: 85 | 86 | ``` 87 | V1 = V4 * C4 * C3 * C2 88 | V4 = V1 / C2 / C3 / C4 89 | ``` 90 | 91 | 4. Convert between parallel coordinate systems: 92 | 93 | ``` 94 | C0 { C1, C2 } 95 | V2 = V1 * C1 / C2 96 | ``` 97 | 98 | 5. More operations: 99 | 100 | Scalar multiplication: 101 | 102 | ``` 103 | C * k = {C.o, C.s * k, C.u} 104 | where: C.u = {C.ux, C.uy, C.uz} 105 | ``` 106 | 107 | Quaternion multiplication: 108 | 109 | ``` 110 | C0 = C1 * q1 111 | C1 = C0 / q1 112 | q0 = q1 * C1 113 | q1 = q0 / C1 114 | ``` 115 | 116 | Vector addition: 117 | 118 | ``` 119 | C2 = C1 + o 120 | Where C2 = {C1.o + o, C1.v}, C1.v = {C1.ux*C1.s.x, C1.uy*C1.s.y, C1.uz*C1.s.z} 121 | ``` 122 | 123 | Coordinate Gradient: 124 | ``` 125 | G = C1 / C2 - I 126 | Where C1 and C2 are coordinate systems on two points on a unit length distance. 127 | ``` 128 | ## Coordinate System Differentiation 129 | 130 | Coord can be used to differentiate coordinate systems in space in three ways: 131 | 132 | Gradient: 133 | 134 | ``` 135 | ▽f = (u * df * Cuv) / Dxyz 136 | Where: 137 | Cuv = {u, v, 0} 138 | Dxyz = {ux * dx, uy * dy, uz * dz} 139 | ``` 140 | 141 | Divergence: 142 | 143 | ``` 144 | ▽ ∙ F = dF / Dxyz ∙ Ic 145 | Where: Ic = {ux, uy, uz} 146 | ``` 147 | 148 | Curl: 149 | 150 | ``` 151 | ▽ x F = dF / Dxyz x Ic 152 | ``` 153 | 154 | ## Differential Geometry Framework 155 | 156 | ### Connection Calculus 157 | ```cpp 158 | // Finite connection between frames 159 | G = C2 / C1 - I; 160 | 161 | // Intrinsic connection (embedded surfaces) 162 | G_intrinsic = C2 / C1 / c2 - I / c1; 163 | ``` 164 | Where: 165 | - `C1,C2` are 3D coordinate frames along the surface 166 | - `c1,c2` are mappings from intrinsic to global coordinates (e.g., cone development coordinates) 167 | 168 | ### Calculate the Space Curvature 169 | Coord can transport vectors from a natural coordinate system to a curved coordinate system in a curved space. The curvature can be determined by comparing two paths projected onto the u and v curves, which is done using Gu and Gv. Gu and Gv represent the gradients of rotational changes of vectors along the u and v. By using a coordinate system, the spatial curvature can be calculated, and the curvature tensor in the u,v coordinate system is given by: 170 | 171 | ``` 172 | Ruv = Gu*Gv - Gv*Gu - G[u,v] 173 | 174 | where: Gu = C2 / C1 - I 175 | Connection vector: [u, v] (Lie bracket operation) 176 | W = Wu + Wv = [u, v] 177 | G[u,v] = Gu*Wu + Gv*Wv 178 | ``` 179 | 180 | ## Lie-Theoretic Interpretation 181 | 182 | The Coord framework naturally embeds Lie theory: 183 | - **Group Multiplication**: Represents SE(3) action 184 | - **Lie Algebra**: The tangent space at identity 185 | - **Bracket Operation**: 186 | ```cpp 187 | [C1, C2] = C1*C2 - C2*C1 188 | ``` 189 | - **Exponential Map**: From algebra to group 190 | 191 | This provides a unified representation for: 192 | - Rigid transformations (SE(3)) 193 | - Conformal transformations 194 | - Gauge transformations 195 | 196 | ## Implementation and Usage 197 | 198 | ## Python Installation 199 | 200 | To use the coordinate_system in Python(3.11), you can easily install it via pip: 201 | 202 | ```bash 203 | pip install coordinate_system 204 | ``` 205 | 206 | ```python 207 | from coordinate_system import vec3,quat,coord3 208 | a = coord3(0,0,1,0,45,0); 209 | b = coord3(1,0,0,45,0,0); 210 | a*=b; 211 | print(a); 212 | ``` 213 | This will allow you to leverage the powerful features of the coord3 class in Python for your mathematical and computational needs. 214 | 215 | ## Computational Advantages 216 | 217 | 1. **Symbolic Clarity**: Matches mathematical notation in code 218 | 2. **Automatic Differentiation**: Built-in differential operations 219 | 3. **Metric Awareness**: Natural handling of scaled/curved spaces 220 | 4. **Type Safety**: Prevents invalid operations at compile time 221 | 222 | ## Conclusion 223 | 224 | The Coord framework provides a unified language for: 225 | - Geometric transformations 226 | - Differential geometry 227 | - Physical reference frames 228 | - Lie-theoretic operations 229 | 230 | By overloading algebraic operations, it creates a computational syntax that mirrors mathematical intuition while remaining efficient for computer implementation. This approach bridges the gap between abstract mathematics and practical computation, particularly in fields requiring rigorous treatment of coordinate systems and their transformations. 231 | 232 | ## Paper online 233 | https://zenodo.org/records/14435614 234 | 235 | ## Code Compilation and Usage 236 | Regarding the compilation and usage of these codes, due to some codes being related to the company's confidentiality policy, I can only release a part of the codes. However, the key points are transparent. You can combine these coordinate system codes with your own vector library for use, or directly use the Python version (currently, it only supports the Windows version). I hope this can be helpful and inspiring to you. 237 | -------------------------------------------------------------------------------- /coord.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************************************************ 2 | * [Coordinate System] 3 | * by Guojun Pan 4 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 5 | * The coordinate system class is separately encapsulated by me for 6 | * simplifying coordinate transformation and deriving many algorithms, 7 | * which can solve some problems related to coordinate system transformation. 8 | * The operation of the coordinate system is similar to Lie group. 9 | * The coordinate system consists of three parts: C = M (position) + S (scaling) * R (rotation). 10 | * 11 | * * * * * * * * * * * * Detailed Explanation * * * * * * * * * * * * * * 12 | * The coordinate system transformation is divided into three steps: 13 | * projection (/), translation (^), and restoration (*). 14 | * 15 | * The symbol of the coordinate system itself is C. The transformation between coordinate systems 16 | * can be written as G = C2 / C1 - I, where G means gradient. 17 | * oper(/) = C1 * C2^-1 18 | * oper(\) = C1^-1 * C2 19 | * 20 | * Specifically: 21 | * Take vectors V1 and V2 at adjacent points (1) and (2) respectively, 22 | * corresponding to coordinate systems C1 and C2. Then: 23 | * V = V1 / C1 = V2 / C2 => 24 | * V2 = V1 * C2 / C1, let R12 = C2 / C1 => 25 | * V2 = V1 * R12 26 | * 27 | * The coordinate system can be used to calculate spatial curvature. In the u,v coordinate system, 28 | * the Curvature tensor is: 29 | * Ruv = Gu*Gv - Gv*Gu - G[u,v] 30 | * where: Gu = C2 / C1 - I 31 | * Connection vector: W = [U, V] (Lie bracket operation) 32 | * G[u,v] = Gu*Wu + Gv*Wv 33 | */ 34 | 35 | //#define NON_UNIFORM_SCALE // For Differential Geometry 36 | // ******************************************************************************************** 37 | // |/_ 38 | // UC 3d Rotation Coordinate System（Base Coordinate System） 39 | // ******************************************************************************************** 40 | struct ucoord3 41 | { 42 | static const ucoord3 ONE; 43 | union { 44 | struct { 45 | // basis 基向量 46 | vec3 ux; 47 | vec3 uy; 48 | vec3 uz; 49 | }; 50 | real m[9]; // 矩阵用法 51 | }; 52 | ucoord3() 53 | { 54 | ux = vec3::UX; uy = vec3::UY; uz = vec3::UZ; 55 | } 56 | ucoord3(const ucoord3& c) 57 | { 58 | ux = c.ux; uy = c.uy; uz = c.uz; 59 | } 60 | ucoord3(const vec3& _ux, const vec3& _uy, const vec3& _uz) 61 | { 62 | ux = _ux; uy = _uy; uz = _uz; 63 | } 64 | ucoord3(const vec3& _ux, const vec3& _uy) 65 | { 66 | ux = _ux; uy = _uy; uz = ux.cross(uy); 67 | } 68 | ucoord3(real ang, const vec3& ax) 69 | { 70 | quaternion q(ang, ax); 71 | ux = q * vec3::UX; 72 | uy = q * vec3::UY; 73 | uz = q * vec3::UZ; 74 | } 75 | ucoord3(real pit, real yaw, real rol) 76 | { 77 | quaternion q(pit, yaw, rol); 78 | ux = q * vec3::UX; 79 | uy = q * vec3::UY; 80 | uz = q * vec3::UZ; 81 | } 82 | ucoord3(const quaternion& q) 83 | { 84 | ux = q * vec3::UX; 85 | uy = q * vec3::UY; 86 | uz = q * vec3::UZ; 87 | } 88 | 89 | // uy方向 推测ux,uz 90 | void fromquat(const quaternion& q) 91 | { 92 | ux = q * vec3::UX; 93 | uy = q * vec3::UY; 94 | uz = q * vec3::UZ; 95 | } 96 | void fromuy(const vec3& _uy) 97 | { 98 | quat q; q.fromvectors(uy, _uy); 99 | fromquat(q); 100 | } 101 | // 引用四元数的欧拉角转化 102 | void frompyr(real pit, real yaw, real rol) 103 | { 104 | fromquat({ pit, yaw, rol }); 105 | } 106 | void frompyr(const vec3& pyr) 107 | { 108 | fromquat(quaternion(pyr.x, pyr.y, pyr.z)); 109 | } 110 | vec3 topyr() const 111 | { 112 | return Q().toeulers(); 113 | } 114 | // 坐标系的欧拉角转化 115 | vec3 toeulers() const 116 | { 117 | return coord2eulers(); 118 | } 119 | // 旋转差 120 | void from_vecs_R(const vec3& v1, const vec3& v2) 121 | { 122 | vec3 v = v1.cross(v2); 123 | real c = v1.dot(v2); 124 | real k = 1.0 / (1.0 + c); 125 | 126 | ux = { v.x * v.x * k + c, v.y * v.x * k - v.z, v.z * v.x * k + v.y }; 127 | uy = { v.x * v.y * k + v.z, v.y * v.y * k + c, v.z * v.y * k - v.x }; 128 | uz = { v.x * v.z * k - v.y, v.y * v.z * k + v.x, v.z * v.z * k + c }; 129 | } 130 | // 轴，向量1，2 131 | void from_ax_vecs(const vec3& ax, const vec3& v1, const vec3& v2) 132 | { 133 | vec3 pv1 = v1.crossdot(ax); 134 | vec3 pv2 = v2.crossdot(ax); 135 | real ang = acos(pv1.dot(pv2)); 136 | quaternion q; q.ang_axis(ang, ax); 137 | fromquat(q); 138 | } 139 | bool same_dirs(const ucoord3& c) const 140 | { 141 | return ux == c.ux && uy == c.uy && uz == c.uz; 142 | } 143 | bool operator == (const ucoord3& c) const 144 | { 145 | return same_dirs(c); 146 | } 147 | bool operator != (const ucoord3& c) const 148 | { 149 | return !same_dirs(c); 150 | } 151 | vec3 operator[] (int index) const 152 | { 153 | if(index == 0) 154 | return ux; 155 | else if (index == 1) 156 | return uy; 157 | else if (index == 2) 158 | return uz; 159 | 160 | return vec3::ZERO; 161 | } 162 | 163 | // 乘法：在坐标系下定义一个向量，或者向量向父空间还原 164 | friend vec3 operator * (const vec3& v, const ucoord3& c) 165 | { 166 | return c.ux * (v.x) + c.uy * (v.y) + c.uz * (v.z); 167 | } 168 | ucoord3 operator * (const ucoord3& c) const 169 | {// C_child * C_parent * ... 170 | ucoord3 rc; 171 | rc.ux = ux.x * c.ux + ux.y * c.uy + ux.z * c.uz; 172 | rc.uy = uy.x * c.ux + uy.y * c.uy + uy.z * c.uz; 173 | rc.uz = uz.x * c.ux + uz.y * c.uy + uz.z * c.uz; 174 | 175 | return rc; 176 | } 177 | friend quaternion operator * (const quaternion& q, const ucoord3& c) 178 | { 179 | return q * c.toquat(); 180 | } 181 | ucoord3 operator * (const quaternion& q) const 182 | { 183 | ucoord3 rc; 184 | rc.ux = q * ux; 185 | rc.uy = q * uy; 186 | rc.uz = q * uz; 187 | return rc; 188 | } 189 | friend void operator *= (vec3& v, const ucoord3& c) 190 | { 191 | v = v * c; 192 | } 193 | void operator *= (const ucoord3& c) 194 | { 195 | *this = (*this) * c; 196 | } 197 | void operator *= (const quaternion& q) 198 | { 199 | ux = q * ux; 200 | uy = q * uy; 201 | uz = q * uz; 202 | } 203 | // 除法：向量向坐标系投影（对于非正交坐标系，建议再扩展） 204 | friend vec3 operator/(const vec3& v, const ucoord3& c) 205 | { 206 | return vec3(v.dot(c.ux), v.dot(c.uy), v.dot(c.uz)); 207 | } 208 | friend void operator/=(vec3& v, const ucoord3& c) 209 | { 210 | v = v / c; 211 | } 212 | // oper(/) = C1 * C2^-1 213 | ucoord3 operator/(const ucoord3& c) const 214 | { 215 | ucoord3 rc; 216 | rc.ux = vec3(ux.dot(c.ux), ux.dot(c.uy), ux.dot(c.uz)); 217 | rc.uy = vec3(uy.dot(c.ux), uy.dot(c.uy), uy.dot(c.uz)); 218 | rc.uz = vec3(uz.dot(c.ux), uz.dot(c.uy), uz.dot(c.uz)); 219 | return rc; 220 | } 221 | void operator/=(const ucoord3& c) 222 | { 223 | *this = (*this) / c; 224 | } 225 | friend quaternion operator / (const quaternion& q, const ucoord3& c) 226 | { 227 | return q * c.toquat().conjcopy(); 228 | } 229 | ucoord3 operator / (const quaternion& q) const 230 | { 231 | return (*this) * q.conjcopy(); 232 | } 233 | void operator /= (const quaternion& q) 234 | { 235 | *this = (*this) / q; 236 | } 237 | // oper(\) = C1^-1 * C2 238 | ucoord3 operator % (const ucoord3& c) const 239 | { 240 | return (*this).reversed() * c; 241 | } 242 | // oper(^) 243 | // 相空间的乘法运算,Ce^(th*v) 244 | // 如C表示某向量A在两点间的旋转， 245 | // 融合向量0(*this); 510 | } 511 | void base(const ucoord3& ucd) 512 | { 513 | ux = ucd.ux; uy = ucd.uy; uz = ucd.uz; 514 | } 515 | const ucoord3& R() const 516 | { 517 | return static_cast(*this); 518 | } 519 | const ucoord3& UC() const 520 | { 521 | return static_cast(*this); 522 | } 523 | void UC(const ucoord3& ucd) 524 | { 525 | ux = ucd.ux; uy = ucd.uy; uz = ucd.uz; 526 | } 527 | 528 | // 乘法：在坐标系下定义一个向量 529 | friend vec3 operator * (const vec3& p, const vcoord3& c) 530 | { 531 | return c.ux * (c.s.x * p.x) + c.uy * (c.s.y * p.y) + c.uz * (c.s.z * p.z); 532 | } 533 | friend void operator *= (vec3& p, const vcoord3& c) 534 | { 535 | p = p * c; 536 | } 537 | vcoord3 operator * (const vec3& v) const 538 | { 539 | return (*this) * vcoord3(vec3::UX * v.x, vec3::UY * v.y, vec3::UZ * v.z); 540 | } 541 | void operator *= (const vec3& v) 542 | { 543 | *this = (*this) * v; 544 | } 545 | friend real operator * (const real& s, const vcoord3& c) 546 | { 547 | return s * ((c.s.x + c.s.y + c.s.z) / 3.0); 548 | } 549 | vcoord3 operator * (real _s) const 550 | { 551 | vcoord3 c = *this; 552 | {// C*S 缩放乘法 553 | c.s *= _s; 554 | } 555 | return c; 556 | } 557 | void operator *= (real _s) 558 | { 559 | *this = (*this) * _s; 560 | } 561 | vcoord3 operator * (const vcoord3& c) const 562 | {// Cchild * Cparent * ... 563 | vcoord3 rc; 564 | #ifdef NON_UNIFORM_SCALE 565 | rc.ux = (ux.x * s.x) * (c.ux * c.s.x) + (ux.y * s.x) * (c.uy * c.s.y) + (ux.z * s.x) * (c.uz * c.s.z); 566 | rc.uy = (uy.x * s.y) * (c.ux * c.s.x) + (uy.y * s.y) * (c.uy * c.s.y) + (uy.z * s.y) * (c.uz * c.s.z); 567 | rc.uz = (uz.x * s.z) * (c.ux * c.s.x) + (uz.y * s.z) * (c.uy * c.s.y) + (uz.z * s.z) * (c.uz * c.s.z); 568 | rc.norm(); 569 | #else 570 | rc = ucoord3::operator*(c); 571 | rc.s = s * c.s; 572 | #endif 573 | return rc; 574 | } 575 | void operator *= (const vcoord3& c) 576 | { 577 | *this = (*this) * c; 578 | } 579 | vcoord3 operator * (const quaternion& q) const 580 | { 581 | vcoord3 rc = *this; 582 | rc.ux = q * ux; 583 | rc.uy = q * uy; 584 | rc.uz = q * uz; 585 | return rc; 586 | } 587 | void operator *= (const quaternion& q) 588 | { 589 | *this = (*this) * q; 590 | } 591 | 592 | // 除法：向量向坐标系投影（对于非正交坐标系，建议再扩展） 593 | friend vec3 operator / (const vec3& v, const vcoord3& c) 594 | { 595 | return vec3(v.dot(c.ux) / c.s.x, v.dot(c.uy) / c.s.y, v.dot(c.uz) / c.s.z); 596 | } 597 | friend void operator /= (vec3& p, const vcoord3& c) 598 | { 599 | p = p / c; 600 | } 601 | vcoord3 operator / (const vec3& v) const 602 | { 603 | return (*this) / vcoord3(vec3::UX * v.x, vec3::UY * v.y, vec3::UZ * v.z); 604 | } 605 | void operator /= (const vec3& v) 606 | { 607 | *this = (*this) / v; 608 | } 609 | 610 | vcoord3 operator / (real _s) const 611 | {// C/S 缩放除法 612 | vcoord3 c = *this; 613 | c.s /= _s; 614 | return c; 615 | } 616 | void operator /= (real _s) 617 | { 618 | *this = (*this) / _s; 619 | } 620 | // oper(/) = C1 * C2^-1 621 | vcoord3 operator / (const vcoord3& c) const 622 | { 623 | vcoord3 rc; 624 | #ifdef NON_UNIFORM_SCALE 625 | vec3 vx = VX(); 626 | vec3 vy = VY(); 627 | vec3 vz = VZ(); 628 | 629 | vec3 cvx = c.ux.normcopy() / c.s.x; 630 | vec3 cvy = c.uy.normcopy() / c.s.y; 631 | vec3 cvz = c.uz.normcopy() / c.s.z; 632 | 633 | rc.ux = vec3(vx.dot(cvx), vx.dot(cvy), vx.dot(cvz)); 634 | rc.uy = vec3(vy.dot(cvx), vy.dot(cvy), vy.dot(cvz)); 635 | rc.uz = vec3(vz.dot(cvx), vz.dot(cvy), vz.dot(cvz)); 636 | 637 | rc.norm(); 638 | #else 639 | rc = ucoord3::operator/(c); 640 | rc.s = s / c.s; 641 | #endif 642 | return rc; 643 | } 644 | void operator /= (const vcoord3& c) 645 | { 646 | *this = (*this) / c; 647 | } 648 | vcoord3 operator / (const quaternion& q) const 649 | { 650 | return (*this) * q.conjcopy(); 651 | } 652 | void operator /= (const quaternion& q) 653 | { 654 | *this = (*this) / q; 655 | } 656 | 657 | // 归一化 658 | void norm(bool bscl = true) 659 | { 660 | s.x = ux.len(); if (!ISZERO(s.x)) ux /= s.x; 661 | s.y = uy.len(); if (!ISZERO(s.y)) uy /= s.y; 662 | s.z = uz.len(); if (!ISZERO(s.z)) uz /= s.z; 663 | if (!bscl) 664 | s = vec3::ONE; 665 | } 666 | vcoord3 normcopy(bool bscl = true) const 667 | { 668 | vcoord3 c = *this; 669 | c.norm(bscl); 670 | return c; 671 | } 672 | 673 | // 倒置 674 | void reverse() 675 | { 676 | (*this) = ONE / (*this); 677 | } 678 | vcoord3 reversed() const 679 | { 680 | return ONE / (*this); 681 | } 682 | 683 | // Cross Product 由电磁场计算引出的叉乘 684 | vcoord3 cross(const vcoord3& c) const 685 | { 686 | vec3 vx = VX(); 687 | vec3 vy = VY(); 688 | vec3 vz = VZ(); 689 | 690 | vec3 cvx = c.VX(); 691 | vec3 cvy = c.VY(); 692 | vec3 cvz = c.VZ(); 693 | 694 | return vcoord3( 695 | vec3::UX * (vy.dot(cvz) - vz.dot(cvy)), 696 | vec3::UY * (vz.dot(cvx) - vx.dot(cvz)), 697 | vec3::UZ * (vx.dot(cvy) - vy.dot(cvx)) 698 | ); 699 | } 700 | // v1 x v2 = v1 * (C x v2) 701 | vcoord3 cross(const vec3& v) const 702 | { 703 | return vcoord3( 704 | VX().cross(v), 705 | VY().cross(v), 706 | VZ().cross(v) 707 | ); 708 | } 709 | 710 | // Dot Product 711 | real dot(const vec3& v) const 712 | { 713 | return v.dot(ux) * s.x + v.dot(uy) * s.y + v.dot(uz) * s.z; 714 | } 715 | real dot(const vcoord3& c) const 716 | { 717 | return c.VX().dot(VX()) + c.VY().dot(VY()) + c.VZ().dot(VZ()); 718 | } 719 | 720 | void dump(const std::string& name = "") const 721 | { 722 | PRINT("----" << name << "---"); 723 | PRINTVEC3(ux); 724 | PRINTVEC3(uy); 725 | PRINTVEC3(uz); 726 | PRINTVEC3(s); 727 | } 728 | }; 729 | #if !defined(PM_IMPLEMENTED) 730 | const vcoord3 vcoord3::ONE = { }; 731 | #endif 732 | 733 | // ****************************************************************** 734 | // |/_ 735 | // C 3d Coordinate System 736 | // ****************************************************************** 737 | struct coord3 : vcoord3 738 | { 739 | static const coord3 ZERO; 740 | static const coord3 ONE; 741 | 742 | union { 743 | vec3 o = vec3::ZERO; // 原点 744 | struct { 745 | real x, y, z; 746 | }; 747 | }; 748 | 749 | coord3() : o(0, 0, 0) {} 750 | coord3(const coord3& other) : o(other.o), vcoord3(other.ux, other.uy, other.uz, other.s) {} 751 | coord3(real x, real y, real z) : o(x, y, z) {} 752 | 753 | coord3(const ucoord3& uc) : vcoord3(uc){} 754 | coord3(const vcoord3& vc) : vcoord3(vc) {} 755 | 756 | coord3(const vec3& _o) : o(_o) {} 757 | coord3(const vec3& _o, const vec3& _s, const vec3& _ux, const vec3& _uy, const vec3& _uz) : vcoord3(_ux, _uy, _uz, _s), o(_o){} 758 | coord3(const vec3& _o, const vec3& _ux, const vec3& _uy, const vec3& _uz) : vcoord3(_ux, _uy, _uz), o(_o){ } 759 | coord3(const vec3& _o, const ucoord3& c) : vcoord3(c), o(_o){} 760 | coord3(const vec3& _o, const vec3& _s, const ucoord3& c) : vcoord3(c, _s), o(_o) {} 761 | 762 | coord3(const vec3& _ux, const vec3& _uy, const vec3& _uz) : vcoord3(_ux, _uy, _uz) {} 763 | coord3(const vec3& _ux, const vec3& _uy) : vcoord3(_ux, _uy, ux.cross(uy)) {} 764 | 765 | coord3(const ucoord3& c,const vec3& _s, const vec3& _o) : vcoord3(c, _s), o(_o){} 766 | coord3(const ucoord3& c,const vec3& _o) : vcoord3(c), o(_o) {} 767 | 768 | coord3(real ang, const vec3& ax) : vcoord3(quaternion(ang, ax)) {} 769 | coord3(const quaternion& q) : vcoord3(q) {} 770 | coord3(const vec3& p, const quaternion& q, const vec3& _s = vec3::ONE) : vcoord3(q, _s), o(p) {} 771 | coord3(real x, real y, real z, real qw, real qx, real qy, real qz, real sx, real sy, real sz) : vcoord3(quaternion(qw, qx, qy, qz), vec3(sx, sy, sz)), o(x, y, z) {} 772 | coord3(real x, real y, real z, real qw, real qx, real qy, real qz) : vcoord3(quaternion(qw, qx, qy, qz), s), o(x, y, z) {} 773 | coord3(real x, real y, real z, real rx, real ry, real rz) 774 | { 775 | real ang2rad = PI / 180.0; 776 | quaternion q(rx * ang2rad, ry * ang2rad, rz * ang2rad); 777 | ux = q * vec3::UX; 778 | uy = q * vec3::UY; 779 | uz = q * vec3::UZ; 780 | o = vec3(x, y, z); 781 | } 782 | 783 | static coord3 from_axes(const vec3& ux, const vec3& uy, const vec3& uz) { 784 | return coord3(vec3::ZERO, vec3::ONE, ux, uy, uz); 785 | } 786 | static coord3 from_angle(real angle, const vec3& axis) { 787 | quaternion q(angle, axis); 788 | return coord3(vec3::ZERO, q); 789 | } 790 | 791 | operator quaternion() const 792 | { 793 | return toquat(); 794 | } 795 | operator vec3() const 796 | { 797 | return o; 798 | } 799 | 800 | vec3 VX() const { return ux * s.x; } 801 | vec3 VY() const { return uy * s.y; } 802 | vec3 VZ() const { return uz * s.z; } 803 | 804 | void VX(const vec3& vx) { real r = vx.len(); ux = vx / r; s.x = r; } 805 | void VY(const vec3& vy) { real r = vy.len(); uy = vy / r; s.y = r; } 806 | void VZ(const vec3& vz) { real r = vz.len(); uz = vz / r; s.z = r; } 807 | 808 | vec3 X() const { return ux * s.x + vec3::UX * o.x; } 809 | vec3 Y() const { return uy * s.y + vec3::UY * o.y; } 810 | vec3 Z() const { return uz * s.z + vec3::UZ * o.z; } 811 | 812 | // 旋转坐标系 813 | const ucoord3& ucoord() const 814 | { 815 | return static_cast(*this); 816 | } 817 | void ucoord(const ucoord3& ucd) 818 | { 819 | ux = ucd.ux; uy = ucd.uy; uz = ucd.uz; 820 | } 821 | void ucoord(vec3 _ux, vec3 _uy, vec3 _uz) 822 | { 823 | ux = _ux; uy = _uy; uz = _uz; 824 | } 825 | const ucoord3& R() const 826 | { 827 | return static_cast(*this); 828 | } 829 | const ucoord3& UC() const 830 | { 831 | return static_cast(*this); 832 | } 833 | void UC(const ucoord3& ucd) 834 | { 835 | ux = ucd.ux; uy = ucd.uy; uz = ucd.uz; 836 | } 837 | void UC(vec3 _ux, vec3 _uy, vec3 _uz) 838 | { 839 | ux = _ux; uy = _uy; uz = _uz; 840 | } 841 | // 向量坐标系 = 方向 X 缩放 842 | const vcoord3& vcoord() const 843 | { 844 | return static_cast(*this); 845 | } 846 | const vcoord3& VC() const 847 | { 848 | return static_cast(*this); 849 | } 850 | // 姿态 851 | coord3 pose() 852 | { 853 | return { ucoord(), vec3::ONE, o }; 854 | } 855 | // 位置 856 | vec3 pos() const 857 | { 858 | return o; 859 | } 860 | // 向量 861 | vec3 tovec() const 862 | { 863 | return ux * s.x + uy * s.y + uz * s.z; 864 | } 865 | coord3 operator=(const coord3& c) 866 | { 867 | o = c.o; 868 | s = c.s; 869 | ux = c.ux; uy = c.uy; uz = c.uz; 870 | return (*this); 871 | } 872 | bool equal_dirs(const coord3& c) const 873 | { 874 | return ux == c.ux && uy == c.uy && uz == c.uz; 875 | } 876 | bool operator==(const coord3& c) const 877 | { 878 | return o == c.o && s == c.s && equal_dirs(c); 879 | } 880 | bool operator!=(const coord3& c) const 881 | { 882 | return o != c.o || s != c.s || !equal_dirs(c); 883 | } 884 | // +/- 运算 885 | coord3 operator+(const coord3& c) const 886 | { 887 | coord3 rc; 888 | vec3 _ux = VX() + c.VX(); 889 | vec3 _uy = VY() + c.VY(); 890 | vec3 _uz = VZ() + c.VZ(); 891 | 892 | rc.s.x = _ux.len(); 893 | if (!ISZERO(rc.s.x)) 894 | { 895 | _ux /= rc.s.x; 896 | rc.ux = _ux; 897 | } 898 | rc.s.y = _uy.len(); 899 | if (!ISZERO(rc.s.y)) 900 | { 901 | _uy /= rc.s.y; 902 | rc.uy = _uy; 903 | } 904 | rc.s.z = _uz.len(); 905 | if (!ISZERO(rc.s.z)) 906 | { 907 | _uz /= rc.s.z; 908 | rc.uz = _uz; 909 | } 910 | 911 | rc.o = o + c.o; 912 | return rc; 913 | } 914 | coord3 operator+=(const coord3& c) 915 | { 916 | *this = (*this) + c; 917 | return *this; 918 | } 919 | coord3 operator+(const vec3& v) const 920 | { 921 | coord3 c = (*this); c.o += v; 922 | return c; 923 | } 924 | coord3 operator+=(const vec3& v) 925 | { 926 | *this = *this + v; 927 | return *this; 928 | } 929 | friend vec3 operator+(const vec3& p, const coord3& c) 930 | { 931 | return p + c.o; 932 | } 933 | friend void operator+=(vec3& p, const coord3& c) 934 | { 935 | p = p + c; 936 | } 937 | friend vec3 operator-(const vec3& p, const coord3& c) 938 | { 939 | return p - c.o; 940 | } 941 | friend void operator-=(vec3& p, const coord3& c) 942 | { 943 | p = p - c; 944 | } 945 | coord3 operator-(const coord3& c) const 946 | { 947 | coord3 rc; 948 | vec3 _ux = VX() - c.VX(); 949 | vec3 _uy = VY() - c.VY(); 950 | vec3 _uz = VZ() - c.VZ(); 951 | 952 | rc.s.x = _ux.len(); 953 | if (!ISZERO(rc.s.x)) 954 | { 955 | _ux /= rc.s.x; 956 | rc.ux = _ux; 957 | } 958 | rc.s.y = _uy.len(); 959 | if (!ISZERO(rc.s.y)) 960 | { 961 | _uy /= rc.s.y; 962 | rc.uy = _uy; 963 | } 964 | rc.s.z = _uz.len(); 965 | if (!ISZERO(rc.s.z)) 966 | { 967 | _uz /= rc.s.z; 968 | rc.uz = _uz; 969 | } 970 | 971 | rc.o = o - c.o; 972 | return rc; 973 | } 974 | coord3 operator-() const 975 | { 976 | coord3 c = (*this); 977 | c.o = -c.o; 978 | return c; 979 | } 980 | coord3 operator-(const vec3& v) const 981 | { 982 | coord3 c = (*this); c.o -= v; 983 | return c; 984 | } 985 | coord3 operator-=(const vec3& v) 986 | { 987 | *this = *this - v; 988 | return *this; 989 | } 990 | 991 | // 乘法：在坐标系下定义一个向量 992 | friend vec3 operator*(const vec3& p, const coord3& c) 993 | { 994 | return c.ux * (c.s.x * p.x) + c.uy * (c.s.y * p.y) + c.uz * (c.s.z * p.z) + c.o; 995 | } 996 | friend void operator*=(vec3& p, const coord3& c) 997 | { 998 | p = p * c; 999 | } 1000 | coord3 operator*(const vec3& v) const 1001 | { 1002 | return (*this) * coord3(vec3::UX * v.x, vec3::UY * v.y, vec3::UZ * v.z); 1003 | } 1004 | void operator*=(const vec3& v) 1005 | { 1006 | *this = (*this) * v; 1007 | } 1008 | coord3 operator*(real _s) const 1009 | { 1010 | coord3 c = *this; 1011 | {// C*S 缩放乘法 1012 | c.s *= _s; 1013 | c.o *= _s; 1014 | } 1015 | return c; 1016 | } 1017 | void operator*=(real _s) 1018 | { 1019 | *this = (*this) * _s; 1020 | } 1021 | coord3 operator*(const coord3& c) const 1022 | {// Cchild * Cparent * ... 1023 | coord3 rc = vcoord3::operator*(c); 1024 | rc.o = c.o + (o.x * c.s.x) * c.ux + (o.y * c.s.y) * c.uy + (o.z * c.s.z) * c.uz; 1025 | return rc; 1026 | } 1027 | coord3 operator*=(const coord3& c) 1028 | { 1029 | *this = (*this) * c; 1030 | return *this; 1031 | } 1032 | coord3 operator*(const vcoord3& c) const 1033 | {// Cchild * Cparent * ... 1034 | coord3 rc = vcoord3::operator*(c); 1035 | rc.o = (o.x * c.s.x) * c.ux + (o.y * c.s.y) * c.uy + (o.z * c.s.z) * c.uz; 1036 | return rc; 1037 | } 1038 | coord3 operator*=(const vcoord3& c) 1039 | { 1040 | *this = (*this) * c; 1041 | return *this; 1042 | } 1043 | coord3 operator*(const ucoord3& c) const 1044 | {// Cchild * Cparent * ... 1045 | coord3 rc = ucoord3::operator*(c); 1046 | rc.o = (o.x) * c.ux + (o.y) * c.uy + (o.z) * c.uz; 1047 | return rc; 1048 | } 1049 | coord3 operator*=(const ucoord3& c) 1050 | { 1051 | *this = (*this) * c; 1052 | return *this; 1053 | } 1054 | coord3 operator*(const quaternion& q) const 1055 | { 1056 | coord3 rc = *this; 1057 | rc.ux = q * ux; 1058 | rc.uy = q * uy; 1059 | rc.uz = q * uz; 1060 | rc.o = q * rc.o; 1061 | return rc; 1062 | } 1063 | coord3 operator*=(const quaternion& q) 1064 | { 1065 | *this = (*this) * q; 1066 | return *this; 1067 | } 1068 | 1069 | // 除法：向量向坐标系投影（对于非正交坐标系，建议再扩展） 1070 | friend vec3 operator/(const vec3& p, const coord3& c) 1071 | { 1072 | vec3 v = p - c.o; 1073 | v = v / c.s; 1074 | return vec3(v.dot(c.ux), v.dot(c.uy), v.dot(c.uz)); 1075 | } 1076 | friend void operator/=(vec3& p, const coord3& c) 1077 | { 1078 | p = p / c; 1079 | } 1080 | coord3 operator/(const vec3& v) const 1081 | { 1082 | return (*this) / coord3(vec3::UX * v.x, vec3::UY * v.y, vec3::UZ * v.z); 1083 | } 1084 | void operator/=(const vec3& v) 1085 | { 1086 | *this = (*this) / v; 1087 | } 1088 | 1089 | coord3 operator/(real _s) const 1090 | {// C/S 缩放除法 1091 | coord3 c = *this; 1092 | c.s /= _s; 1093 | c.o /= _s; 1094 | return c; 1095 | } 1096 | void operator/=(real _s) 1097 | { 1098 | *this = (*this) / _s; 1099 | } 1100 | // oper(/) = C1 * C2^ - 1 1101 | coord3 operator/(const coord3& c) const 1102 | { 1103 | coord3 rc = vcoord3::operator/(c); 1104 | rc.o = o - c.o; 1105 | rc.o = vec3(rc.o.dot(c.ux) / c.s.x, rc.o.dot(c.uy) / c.s.y, rc.o.dot(c.uz) / c.s.z); 1106 | return rc; 1107 | } 1108 | coord3 operator/=(const coord3& c) 1109 | { 1110 | *this = (*this) / c; 1111 | return *this; 1112 | } 1113 | coord3 operator/(const vcoord3& c) const 1114 | { 1115 | coord3 rc = vcoord3::operator/(c); 1116 | rc.o = o; 1117 | rc.o = vec3(rc.o.dot(c.ux) / c.s.x, rc.o.dot(c.uy) / c.s.y, rc.o.dot(c.uz) / c.s.z); 1118 | return rc; 1119 | } 1120 | coord3 operator/=(const vcoord3& c) 1121 | { 1122 | *this = (*this) / c; 1123 | return *this; 1124 | } 1125 | coord3 operator/(const ucoord3& c) const 1126 | { 1127 | coord3 rc = ucoord3::operator/(c); 1128 | rc.o = o; 1129 | rc.o = vec3(rc.o.dot(c.ux), rc.o.dot(c.uy), rc.o.dot(c.uz)); 1130 | return rc; 1131 | } 1132 | coord3 operator/=(const ucoord3& c) 1133 | { 1134 | *this = (*this) / c; 1135 | return *this; 1136 | } 1137 | coord3 operator/(const quaternion& q) const 1138 | { 1139 | return (*this) * q.conjcopy(); 1140 | } 1141 | void operator/=(const quaternion& q) 1142 | { 1143 | *this = (*this) / q; 1144 | } 1145 | // oper(\) = C1^-1 * C2 1146 | coord3 operator%(const coord3& c) const 1147 | { 1148 | return (*this).reversed() * c; 1149 | } 1150 | coord3 operator^(const vec3& v) const 1151 | { 1152 | coord3 c = *this; 1153 | c.ux = vec3::lerp(vec3::UX, c.ux, v.x); c.ux.norm(); 1154 | c.uy = vec3::lerp(vec3::UY, c.uy, v.y); c.uy.norm(); 1155 | c.uz = vec3::lerp(vec3::UZ, c.uz, v.z); c.uz.norm(); 1156 | 1157 | c.s = vec3::lerp(vec3::ONE, c.s, v); 1158 | c.o = vec3::lerp(vec3::ZERO, c.o, v); 1159 | 1160 | return c; 1161 | } 1162 | coord3 operator ^ (real t) const 1163 | { 1164 | ucoord3 uc = ucoord(); 1165 | uc ^= t; 1166 | /*c.ux = vec3::lerp(vec3::UX, c.ux, t); c.ux.norm(); 1167 | c.uy = vec3::lerp(vec3::UY, c.uy, t); c.uy.norm(); 1168 | c.uz = vec3::lerp(vec3::UZ, c.uz, t); c.uz.norm();*/ 1169 | 1170 | vec3 _s = vec3::lerp(vec3::ONE, s, t); 1171 | vec3 _o = vec3::lerp(vec3::ZERO, o, t); 1172 | 1173 | return coord3(uc, _s, _o); 1174 | } 1175 | // 倒置 1176 | void reverse() 1177 | { 1178 | (*this) = ONE / (*this); 1179 | } 1180 | coord3 reversed() const 1181 | { 1182 | return ONE / (*this); 1183 | } 1184 | // 由李符号引出的叉乘，更加符合群论 1185 | coord3 lie_cross(const coord3& c) const 1186 | { 1187 | return (*this) * c - c * (*this); 1188 | } 1189 | // 梯度坐标系 1190 | // V2 = V1 * (C2 / C1 - I) 1191 | // G = C2 / C1 - I 1192 | static coord3 grad(const coord3& c1, const coord3& c2) 1193 | { 1194 | return c2 / c1 - ONE; 1195 | } 1196 | std::string serialise() const 1197 | { 1198 | vec3 eu = coord2eulers(); 1199 | return o.serialise() + "," + eu.serialise(); 1200 | } 1201 | void dump(const std::string& name = "") const 1202 | { 1203 | PRINT("|/_ : " << name); 1204 | PRINTVEC3(ux); 1205 | PRINTVEC3(uy); 1206 | PRINTVEC3(uz); 1207 | PRINTVEC3(s); 1208 | PRINTVEC3(o); 1209 | } 1210 | }; 1211 | #if !defined(PM_IMPLEMENTED) 1212 | const coord3 coord3::ZERO = {ucoord3::ONE, vec3::ZERO, vec3::ZERO }; 1213 | const coord3 coord3::ONE = {}; 1214 | #endif 1215 | -------------------------------------------------------------------------------- /coord2.hpp: -------------------------------------------------------------------------------- 1 | /************************************************************************************************ 2 | * [Coordinate System] 3 | * by Guojun Pan 4 | * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 5 | * The coordinate system class is separately encapsulated by me for 6 | * simplifying coordinate transformation and deriving many algorithms, 7 | * which can solve some problems related to coordinate system transformation. 8 | * The operation of the coordinate system is similar to Lie group. 9 | * The coordinate system consists of three parts: C = M (position) + S (scaling) * R (rotation). 10 | * 11 | * * * * * * * * * * * * Detailed Explanation * * * * * * * * * * * * * * 12 | * The coordinate system transformation is divided into three steps: 13 | * projection (/), translation (^), and restoration (*). 14 | * 15 | * The symbol of the coordinate system itself is C. The transformation between coordinate systems 16 | * can be written as G = C2 / C1 - I, where G means gradient. 17 | * oper(/) = C1 * C2^-1 18 | * oper(\) = C1^-1 * C2 19 | * 20 | * Specifically: 21 | * Take vectors V1 and V2 at adjacent points (1) and (2) respectively, 22 | * corresponding to coordinate systems C1 and C2. Then: 23 | * V = V1 / C1 = V2 / C2 => 24 | * V2 = V1 * C2 / C1, let R12 = C2 / C1 => 25 | * V2 = V1 * R12 26 | * 27 | * The coordinate system can be used to calculate spatial curvature. In the u,v coordinate system, 28 | * the Curvature tensor is: 29 | * Ruv = Gu*Gv - Gv*Gu - G[u,v] 30 | * where: Gu = C2 / C1 - I 31 | * Connection vector: W = [U, V] (Lie bracket operation) 32 | * G[u,v] = Gu*Wu + Gv*Wv 33 | */ 34 | 35 | //#define NON_UNIFORM_SCALE 36 | // ******************************************************************* 37 | // |_ 38 | // UC 2d Rotation Coordinate System 39 | // ******************************************************************* 40 | struct ucoord2 41 | { 42 | static const ucoord2 ZERO; 43 | static const ucoord2 ONE; 44 | 45 | vec2 ux = vec2::UX; // basis 单位化基向量 46 | vec2 uy = vec2::UY; 47 | 48 | ucoord2() {} 49 | ucoord2(crvec2 _ux, crvec2 _uy) 50 | { 51 | ux = _ux; 52 | uy = _uy; 53 | } 54 | ucoord2(crvec2 _ux) 55 | { 56 | ux = _ux; 57 | uy = ux.rotcopy(PI / 2); 58 | } 59 | ucoord2(real ang) 60 | { 61 | ux.rot(ang); 62 | uy.rot(ang); 63 | } 64 | bool is_same_dirs(const ucoord2& c) const 65 | { 66 | return ux == c.ux && uy == c.uy; 67 | } 68 | // 在坐标系下定义一个向量 69 | friend vec2 operator * (crvec2 p, const ucoord2& c) 70 | { 71 | return c.ux * (p.x) + c.uy * (p.y); 72 | } 73 | ucoord2 operator * (const ucoord2& c) const 74 | { 75 | ucoord2 rc; 76 | rc.ux = ux.x * c.ux + ux.y * c.uy; 77 | rc.uy = uy.x * c.ux + uy.y * c.uy; 78 | return rc; 79 | } 80 | 81 | // 向量向坐标系投影 82 | friend vec2 operator / (crvec2 v, const ucoord2& c) 83 | { 84 | return vec2(v.dot(c.ux), v.dot(c.uy)); 85 | } 86 | // oper(/) = C1 * C2^-1 87 | ucoord2 operator / (const ucoord2& c) const 88 | { 89 | ucoord2 rc; 90 | rc.ux = vec2(ux.dot(c.ux), ux.dot(c.uy)); 91 | rc.uy = vec2(uy.dot(c.ux), uy.dot(c.uy)); 92 | return rc; 93 | } 94 | // oper(//) = C1^-1 * C2 95 | ucoord2 operator % (const ucoord2& c) const 96 | { 97 | return (*this).reversed() * c; 98 | } 99 | // 倒置 100 | void reverse() 101 | { 102 | (*this) = ONE / (*this); 103 | } 104 | ucoord2 reversed() const 105 | { 106 | return ONE / (*this); 107 | } 108 | // 梯度坐标系 109 | static ucoord2 grad(const ucoord2& c1, const ucoord2& c2) 110 | { 111 | return c1.reversed() * c2; 112 | } 113 | real dot(crvec2 v) const 114 | { 115 | return v.dot(ux) + v.dot(uy); 116 | } 117 | 118 | // 角度 119 | real angle() const 120 | { 121 | return ux.angle(); 122 | } 123 | // 旋转 124 | void rot(real angle) 125 | { 126 | vec2 z = vector2::ang_len(angle, 1); 127 | ux = complex_mul(ux, z); 128 | uy = complex_mul(uy, z); 129 | } 130 | ucoord2 rotcopy(real angle) const 131 | { 132 | ucoord2 c = (*this); 133 | vec2 z = vector2::ang_len(angle, 1); 134 | c.ux = complex_mul(c.ux, z); 135 | c.uy = complex_mul(c.uy, z); 136 | return c; 137 | } 138 | void rot2dir(crvec2 _dir) 139 | { 140 | ux = _dir; uy = _dir.rotcopy(PI / 2); 141 | } 142 | void dump(const std::string& name = "") const 143 | { 144 | PRINT("----" << name << "---"); 145 | PRINTVEC2(ux); 146 | PRINTVEC2(uy); 147 | } 148 | }; 149 | #if defined(PMDLL) || !defined(PM_IMPLEMENTED) 150 | const ucoord2 ucoord2::ZERO = { 0 }; 151 | const ucoord2 ucoord2::ONE = ucoord2(); 152 | #endif 153 | 154 | // ******************************************************************* 155 | // |_ 156 | // C 2d Coordinate System 157 | // ******************************************************************* 158 | struct coord2 : ucoord2 159 | { 160 | static const coord2 ZERO; 161 | static const coord2 ONE; 162 | 163 | vec2 s = vec2::ONE; // 缩放 164 | vec2 o; // 原点 165 | 166 | coord2() {} 167 | coord2(const ucoord2& c) : ucoord2(c) 168 | { 169 | } 170 | coord2(const ucoord2& c, crvec2 _s) : ucoord2(c) 171 | { 172 | s = _s; 173 | } 174 | coord2(const coord2& c) : ucoord2(c.ux, c.uy) 175 | { 176 | s = c.s; 177 | o = c.o; 178 | } 179 | coord2(crvec2 _ux, crvec2 _uy) : ucoord2(_ux, _uy) 180 | { 181 | } 182 | coord2(crvec2 p) 183 | { 184 | o = p; 185 | } 186 | coord2(real ang) 187 | { 188 | vec2 z = vector2::ang_len(ang, 1); 189 | ux = complex_mul(ux, z); 190 | uy = complex_mul(uy, z); 191 | } 192 | coord2(real ang, real _r) 193 | { 194 | vec2 z = vector2::ang_len(ang, 1); 195 | ux = complex_mul(ux, z); 196 | uy = complex_mul(uy, z); 197 | s *= _r; 198 | } 199 | coord2(crvec2 p, real ang) 200 | { 201 | o = p; 202 | vec2 z = vector2::ang_len(ang, 1); 203 | ux = complex_mul(ux, z); 204 | uy = complex_mul(uy, z); 205 | } 206 | coord2(crvec2 p, crvec2 _s, real ang) 207 | { 208 | o = p; 209 | s = _s; 210 | 211 | vec2 z = vector2::ang_len(ang, 1); 212 | ux = complex_mul(ux, z); 213 | uy = complex_mul(uy, z); 214 | } 215 | operator vec2 () const 216 | { 217 | return o; 218 | } 219 | vec2 VX() const { return ux * s.x; } 220 | vec2 VY() const { return uy * s.y; } 221 | coord2 VC() const 222 | { 223 | return { VX(), VY() }; 224 | } 225 | ucoord2 UC() const 226 | { 227 | return {ux, uy}; 228 | } 229 | void UC(const ucoord2& uc) 230 | { 231 | ux = uc.ux; 232 | uy = uc.uy; 233 | } 234 | bool equal_dirs(const coord2& c) const 235 | { 236 | return ux == c.ux && uy == c.uy && o == c.o && s == c.s; 237 | } 238 | bool operator == (const coord2& c) const 239 | { 240 | return o == c.o && s == c.s && equal_dirs(c); 241 | } 242 | coord2 operator + (const coord2& c) const 243 | { 244 | coord2 rc; 245 | rc.ux = VX() + c.VX(); 246 | rc.uy = VY() + c.VY(); 247 | rc.norm(); 248 | rc.o = o + c.o; 249 | return rc; 250 | } 251 | void operator += (const coord2& c) 252 | { 253 | *this = *this + c; 254 | } 255 | coord2 operator + (const vec2& v) const 256 | { 257 | coord2 c = (*this); 258 | c.o += v; 259 | return c; 260 | } 261 | void operator += (const vec2& v) 262 | { 263 | *this = *this + v; 264 | } 265 | coord2 operator - (const coord2& c) const 266 | { 267 | coord2 rc; 268 | rc.ux = VX() - c.VX(); 269 | rc.uy = VY() - c.VY(); 270 | rc.norm(); 271 | rc.o = o - c.o; 272 | return rc; 273 | } 274 | void operator -= (const coord2& c) 275 | { 276 | *this = *this - c; 277 | } 278 | coord2 operator - (const vec2& v) const 279 | { 280 | coord2 c = (*this); 281 | c.o -= v; 282 | return c; 283 | } 284 | void operator -= (const vec2& v) 285 | { 286 | *this = *this - v; 287 | } 288 | // 在坐标系下定义一个向量 289 | friend vec2 operator * (crvec2 p, const coord2& c) 290 | { 291 | return c.ux * (c.s.x * p.x) + c.uy * (c.s.y * p.y) + c.o; 292 | } 293 | coord2 operator * (crvec2 v) const 294 | { 295 | return (*this) * coord2(vec2::UX * v.x, vec2::UY * v.y); 296 | } 297 | void operator *= (crvec2 v) 298 | { 299 | (*this) *= coord2(vec2::UX * v.x, vec2::UY * v.y); 300 | } 301 | coord2 operator * (const coord2& c) const 302 | { 303 | coord2 rc; 304 | #ifdef NON_UNIFORM_SCALE 305 | rc.ux = (ux.x * s.x) * (c.ux * c.s.x) + (ux.y * s.x) * (c.uy * c.s.y); 306 | rc.uy = (uy.x * s.y) * (c.ux * c.s.x) + (uy.y * s.y) * (c.uy * c.s.y); 307 | rc.norm(); 308 | #else 309 | rc = ucoord2::operator*(c); 310 | rc.s = s * c.s; 311 | #endif 312 | rc.o = c.o + o.x * c.s.x * c.ux + o.y * c.s.y * c.uy; 313 | return rc; 314 | } 315 | void operator *= (const coord2& c) 316 | { 317 | (*this) = (*this) * c; 318 | } 319 | coord2 operator * (real s) const 320 | { 321 | coord2 c = *this; 322 | //{// C*S 缩放乘法 323 | // c.s.x *= s; c.s.y *= s; 324 | //} 325 | {// C*S 移动乘法 326 | c.o *= s; 327 | } 328 | return c; 329 | } 330 | void operator *= (real s) 331 | { 332 | *this = (*this) * s; 333 | } 334 | // 向量向坐标系投影 335 | friend vec2 operator / (crvec2 p, const coord2& c) 336 | { 337 | vec2 v = p - c.o; 338 | v /= c.s; 339 | return vec2(v.dot(c.ux), v.dot(c.uy)); 340 | } 341 | // oper(/) = C1 * C2^-1 342 | coord2 operator / (const coord2& c) const 343 | { 344 | coord2 rc; 345 | #ifdef NON_UNIFORM_SCALE 346 | vec2 vx = VX(); 347 | vec2 vy = VY(); 348 | 349 | vec2 cvx = c.ux / c.s.x; 350 | vec2 cvy = c.uy / c.s.y; 351 | 352 | rc.ux = vec2(vx.dot(cvx), vx.dot(cvy)); 353 | rc.uy = vec2(vy.dot(cvx), vy.dot(cvy)); 354 | 355 | rc.norm(); 356 | #else 357 | rc = ucoord2::operator/(c); 358 | rc.s = s / c.s; 359 | #endif 360 | rc.o = o - c.o; 361 | rc.o = vec2(rc.o.dot(c.ux) / c.s.x, rc.o.dot(c.uy) / c.s.y); 362 | return rc; 363 | } 364 | coord2 operator / (crvec2 v) const 365 | { 366 | return (*this) / coord2(ux * v.x, uy * v.y); 367 | } 368 | // oper(//) = C1^-1 * C2 369 | coord2 operator % (const coord2& c) const 370 | { 371 | return (*this).reversed() * c; 372 | } 373 | coord2 operator ^ (real f) const 374 | { 375 | real ang = ux.angle() * f; 376 | real rad = ::exp(::log(ux.len()) * f); 377 | return coord2(ang, rad); 378 | } 379 | void norm(bool bscl = true) 380 | { 381 | s.x = ux.len(); if (!ISZERO(s.x)) ux /= s.x; 382 | s.y = uy.len(); if (!ISZERO(s.y)) uy /= s.y; 383 | 384 | if (!bscl) 385 | s = vec2::ONE; 386 | } 387 | // 倒置 388 | void reverse() 389 | { 390 | (*this) = ONE / (*this); 391 | } 392 | coord2 reversed() const 393 | { 394 | return ONE / (*this); 395 | } 396 | real dot(crvec2 v) const 397 | { 398 | return v.dot(ux) * s.x + v.dot(uy) * s.y; 399 | } 400 | // 梯度 401 | static coord2 grad(const coord2& c1, const coord2& c2) 402 | { 403 | return c1.reversed() * c2 - ONE; 404 | } 405 | // 位置 406 | vec2 pos() const 407 | { 408 | return o; 409 | } 410 | string serialise() const 411 | { 412 | return o.serialise() + "," + std::to_string(angle()); 413 | } 414 | void dump(const string& name = "") const 415 | { 416 | PRINT("----" << name << "---"); 417 | PRINTVEC2(ux); 418 | PRINTVEC2(uy); 419 | PRINTVEC2(s); 420 | PRINTVEC2(o); 421 | } 422 | }; 423 | #ifndef(PM_IMPLEMENTED) 424 | const coord2 coord2::ZERO = { ucoord2::ZERO, vec2::ZERO }; 425 | const coord2 coord2::ONE = coord2(); 426 | #endif 427 | -------------------------------------------------------------------------------- /lorentz_coord.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * [Lorentz Coordinate System] 3 | * 4 | * The Lorentz Coordinate System is a coordinate system designed to describe space-time transformations and handle the time dimension. 5 | * It is an extension of the complex coordinate system, where the rotation of the coordinate system can be represented by a quaternion composed of a vector and a complex angle. 6 | * The Lorentz Coordinate System consists of three vectors, ux, uy, and uz, which represent the x, y, and z-axis directions respectively. 7 | * Additionally, it includes a complex number power, which represents the power in the time dimension. 8 | * The Lorentz Coordinate System allows for operations such as multiplication, division, cross product, transpose, reverse, flip, rotation, and conversion to Euler angles. 9 | * These operations enable the transformation of vectors between different coordinate systems and the calculation of relativistic effects. 10 | * The design philosophy of the Lorentz Coordinate System is to provide a flexible and intuitive way to handle space-time transformations and relativistic effects. 11 | * By incorporating complex numbers and quaternions, it allows for a more comprehensive representation of rotations and transformations in 3D space. 12 | */ 13 | 14 | struct lorentz_coord 15 | { 16 | static const lorentz_coord ZERO; 17 | static const lorentz_coord ONE; 18 | vec3 ux = vec3::UX; // x-axis direction 19 | vec3 uy = vec3::UY; // y-axis direction 20 | vec3 uz = vec3::UZ; // z-axis direction 21 | complex power; // power in the time dimension 22 | 23 | lorentz_coord() {} 24 | 25 | /** 26 | * Constructor 27 | * @param c - lorentz_coord object 28 | */ 29 | lorentz_coord(const lorentz_coord& c) 30 | { 31 | ux = c.ux; uy = c.uy; uz = c.uz; 32 | power = c.power; 33 | } 34 | 35 | /** 36 | * Constructor 37 | * @param _ux - x-axis direction 38 | * @param _uy - y-axis direction 39 | * @param _uz - z-axis direction 40 | * @param _power - power in the time dimension 41 | */ 42 | lorentz_coord(const vec3& _ux, const vec3& _uy, const vec3& _uz, const complex& _power) 43 | { 44 | ux = _ux; uy = _uy; uz = _uz; 45 | power = _power; 46 | } 47 | 48 | /** 49 | * Constructor 50 | * @param _ux - x-axis direction 51 | * @param _uy - y-axis direction 52 | * @param _power - power in the time dimension 53 | */ 54 | lorentz_coord(const vec3& _ux, const vec3& _uy, const complex& _power) 55 | { 56 | ux = _ux; uy = _uy; uz = ux.cross(uy); 57 | power = _power; 58 | } 59 | 60 | /** 61 | * Constructor 62 | * @param ang - angle 63 | * @param ax - axis 64 | * @param _power - power in the time dimension 65 | */ 66 | lorentz_coord(real ang, const vec3& ax, const complex& _power) 67 | { 68 | ux.rot(ang, ax); 69 | uy.rot(ang, ax); 70 | uz.rot(ang, ax); 71 | power = _power; 72 | } 73 | 74 | /** 75 | * Constructor 76 | * @param pit - pitch 77 | * @param yaw - yaw 78 | * @param rol - roll 79 | * @param _power - power in the time dimension 80 | */ 81 | lorentz_coord(real pit, real yaw, real rol, const complex& _power) 82 | { 83 | ux.rot(pit, vec3::UX); 84 | uy.rot(yaw, vec3::UY); 85 | uz.rot(rol, vec3::UZ); 86 | power = _power; 87 | } 88 | 89 | /** 90 | * Constructor 91 | * @param q - quaternion 92 | * @param _power - power in the time dimension 93 | */ 94 | lorentz_coord(const quaternion& q, const complex& _power) 95 | { 96 | ux = q * vec3::UX; 97 | uy = q * vec3::UY; 98 | uz = q * vec3::UZ; 99 | power = _power; 100 | } 101 | 102 | /** 103 | * Multiplication: define a vector in the coordinate system or restore a vector to the parent space 104 | * @param p - vector 105 | * @param c - lorentz_coord object 106 | * @return result of the multiplication 107 | */ 108 | friend vec3 operator * (const vec3& p, const lorentz_coord& c) 109 | { 110 | return (c.ux * p.x + c.uy * p.y + c.uz * p.z) * cos(c.power); 111 | } 112 | friend vec4 operator * (const vec4& p, const lorentz_coord& c) 113 | { 114 | return vec4((c.ux * p.x + c.uy * p.y + c.uz * p.z) * cos(c.power), p.w + c.power); 115 | } 116 | /** 117 | * Multiplication: Cchild * Cparent * ... 118 | * @param c - lorentz_coord object 119 | * @return result of the multiplication 120 | */ 121 | lorentz_coord operator * (const lorentz_coord& c) const 122 | { 123 | lorentz_coord rc; 124 | rc.ux = ux.x * c.ux + ux.y * c.uy + ux.z * c.uz; 125 | rc.uy = uy.x * c.ux + uy.y * c.uy + uy.z * c.uz; 126 | rc.uz = uz.x * c.ux + uz.y * c.uy + uz.z * c.uz; 127 | rc.power = power + c.power; 128 | return rc; 129 | } 130 | 131 | /** 132 | * Multiplication: q * C 133 | * @param q - quaternion 134 | * @param c - lorentz_coord object 135 | * @return result of the multiplication 136 | */ 137 | friend quaternion operator * (const quaternion& q, const lorentz_coord& c) 138 | { 139 | return q * c.to_quaternion(); 140 | } 141 | 142 | /** 143 | * Multiplication: C * q 144 | * @param q - quaternion 145 | * @return result of the multiplication 146 | */ 147 | lorentz_coord operator * (const quaternion& q) const 148 | { 149 | lorentz_coord rc; 150 | rc.ux = q * ux; 151 | rc.uy = q * uy; 152 | rc.uz = q * uz; 153 | rc.power = power + q.angle(); 154 | return rc; 155 | } 156 | 157 | /** 158 | * Division: vector projection onto the coordinate system 159 | * @param v - vector 160 | * @param c - lorentz_coord object 161 | * @return result of the division 162 | */ 163 | friend vec3 operator / (const vec3& v, const lorentz_coord& c) 164 | { 165 | return vec3(v.dot(c.ux), v.dot(c.uy), v.dot(c.uz)) * cos(-c.power); 166 | } 167 | 168 | /** 169 | * Division: C1 * C2^-1 170 | * @param c - lorentz_coord object 171 | * @return result of the division 172 | */ 173 | lorentz_coord operator / (const lorentz_coord& c) const 174 | { 175 | lorentz_coord rc; 176 | rc.ux = vec3(ux.dot(c.ux), ux.dot(c.uy), ux.dot(c.uz)); 177 | rc.uy = vec3(uy.dot(c.ux), uy.dot(c.uy), uy.dot(c.uz)); 178 | rc.uz = vec3(uz.dot(c.ux), uz.dot(c.uy), uz.dot(c.uz)); 179 | rc.power = power - c.power; 180 | return rc; 181 | } 182 | 183 | /** 184 | * Division: q / C 185 | * @param q - quaternion 186 | * @param c - lorentz_coord object 187 | * @return result of the division 188 | */ 189 | friend quaternion operator / (const quaternion& q, const lorentz_coord& c) 190 | { 191 | return q * c.to_quaternion().conjugate(); 192 | } 193 | 194 | /** 195 | * Division: C / q 196 | * @param q - quaternion 197 | * @return result of the division 198 | */ 199 | lorentz_coord operator / (const quaternion& q) const 200 | { 201 | return (*this) * q.conjugate(); 202 | } 203 | 204 | /** 205 | * Cross product: C1 x C2 206 | * @param c - lorentz_coord object 207 | * @return result of the cross product 208 | */ 209 | lorentz_coord cross(const lorentz_coord& c) const 210 | { 211 | return lorentz_coord( 212 | ux.cross(c.uz) - ux.cross(c.uy), 213 | uy.cross(c.ux) - uy.cross(c.uz), 214 | uz.cross(c.uy) - uz.cross(c.ux), 215 | power + c.power 216 | ); 217 | } 218 | 219 | /** 220 | * Cross product: C x v 221 | * @param v - vector 222 | * @return result of the cross product 223 | */ 224 | lorentz_coord cross(const vec3& v) const 225 | { 226 | return lorentz_coord( 227 | ux.cross(v), 228 | uy.cross(v), 229 | uz.cross(v), 230 | power 231 | ); 232 | } 233 | /** 234 | * Reverse 235 | */ 236 | void reverse() 237 | { 238 | (*this) = ONE / (*this); 239 | } 240 | 241 | /** 242 | * Reverse 243 | * @return reversed lorentz_coord object 244 | */ 245 | lorentz_coord reversed() const 246 | { 247 | return ONE / (*this); 248 | } 249 | /** 250 | * Rotation 251 | * @param ang - angle 252 | * @param ax - axis 253 | */ 254 | void rotate(real ang, const vec3& ax) 255 | { 256 | ux.rotate(ang, ax); 257 | uy.rotate(ang, ax); 258 | uz.rotate(ang, ax); 259 | } 260 | 261 | /** 262 | * Convert coordinate system to Euler angles 263 | * @return Euler angles 264 | */ 265 | vec3 coord_to_eulers() const 266 | { 267 | const lorentz_coord& rm = *this; 268 | float sy = sqrt(rm.ux.x * rm.ux.x + rm.uy.x * rm.uy.x); 269 | bool singular = sy < 1e-6; 270 | 271 | float x, y, z; 272 | if (!singular) 273 | { 274 | x = atan2(rm.uz.y, rm.uz.z); 275 | y = atan2(-rm.uz.x, sy); 276 | z = atan2(rm.uy.x, rm.ux.x); 277 | } 278 | else 279 | { 280 | x = atan2(-rm.uy.z, rm.uy.y); 281 | y = atan2(-rm.uz.x, sy); 282 | z = 0; 283 | } 284 | return vec3(x, y, z); 285 | } 286 | 287 | /** 288 | * Dot product with a vector 289 | * @param v - vector 290 | * @return dot product 291 | */ 292 | real dot(const vec3& v) const 293 | { 294 | return v.dot(ux) + v.dot(uy) + v.dot(uz); 295 | } 296 | 297 | /** 298 | * Dot product with another coordinate system 299 | * @param c - lorentz_coord object 300 | * @return dot product 301 | */ 302 | real dot(const lorentz_coord& c) const 303 | { 304 | return c.ux.dot(ux) + c.uy.dot(uy) + c.uz.dot(uz); 305 | } 306 | 307 | /** 308 | * Gradient coordinate system = Gradient X Tangent space 309 | * @param c1 - initial lorentz_coord object 310 | * @param c2 - transformed lorentz_coord object 311 | * @return gradient lorentz_coord object 312 | */ 313 | lorentz_coord gradient(const lorentz_coord& c1, const lorentz_coord& c2) 314 | { 315 | return c1.reversed() * c2; 316 | } 317 | 318 | /** 319 | * Convert to quaternion 320 | * @return quaternion 321 | */ 322 | quaternion to_quaternion() const 323 | { 324 | vec3 pyr = coord_to_eulers(); 325 | quaternion q; 326 | q.from_euler(pyr.x, pyr.y, pyr.z); 327 | return q; 328 | } 329 | }; 330 | 331 | const lorentz_coord lorentz_coord::ZERO = lorentz_coord(vec3::ZERO, vec3::ZERO, vec3::ZERO, complex::ZERO); 332 | const lorentz_coord lorentz_coord::ONE = lorentz_coord(vec3::UX, vec3::UY, vec3::UZ, complex::ONE); 333 | -------------------------------------------------------------------------------- /quaternion.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | [Quaternion] 3 | Quaternions are an extension of complex numbers. Unit quaternions are used 4 | for rotation operations. Vectors are derived from quaternions, but there are 5 | differences between the two. Currently, the relationship and application 6 | between quaternions and vectors are still under debate. 7 | ********************************************************************************/ 8 | struct quaternion 9 | { 10 | static const quaternion ONE; 11 | static const quaternion UX; 12 | static const quaternion UY; 13 | static const quaternion UZ; 14 | 15 | real w = 1, x = 0, y = 0, z = 0; 16 | 17 | //----------------------------------------------------------------------- 18 | quaternion() { } 19 | quaternion( 20 | real fW, 21 | real fX, real fY, real fZ) 22 | { 23 | w = fW; 24 | x = fX; 25 | y = fY; 26 | z = fZ; 27 | } 28 | quaternion(real pitch, real yaw, real roll) 29 | { 30 | from_eulers(pitch, yaw, roll); 31 | } 32 | quaternion(const vector3& pyr) 33 | { 34 | from_eulers(pyr.x, pyr.y, pyr.z); 35 | } 36 | quaternion(const quaternion& rkQ) 37 | { 38 | w = rkQ.w; 39 | x = rkQ.x; 40 | y = rkQ.y; 41 | z = rkQ.z; 42 | } 43 | quaternion(real rfAngle, const vector3& rkAxis) 44 | { 45 | ang_axis(rfAngle, rkAxis); 46 | } 47 | quaternion(const vec3& v1, const vec3& v2) 48 | { 49 | from_vectors(v1, v2); 50 | } 51 | 52 | //----------------------------------------------------------------------- 53 | bool is_finite() const 54 | { 55 | return std::isfinite(x) && std::isfinite(y) && std::isfinite(z) && std::isfinite(w); 56 | } 57 | 58 | //----------------------------------------------------------------------- 59 | quaternion operator+ (const quaternion& rkQ) const 60 | { 61 | return quaternion(w + rkQ.w, x + rkQ.x, y + rkQ.y, z + rkQ.z); 62 | } 63 | 64 | quaternion operator- (const quaternion& rkQ) const 65 | { 66 | return quaternion(w - rkQ.w, x - rkQ.x, y - rkQ.y, z - rkQ.z); 67 | } 68 | quaternion operator - () const 69 | { 70 | quaternion q; 71 | q.x = -x; 72 | q.y = -y; 73 | q.z = -z; 74 | q.w = -w; 75 | return q; 76 | } 77 | 78 | //----------------------------------------------------------------------- 79 | vector3 operator* (const vector3& v) const 80 | { 81 | // nVidia SDK implementation 82 | vector3 uv, uuv; 83 | vector3 qvec(x, y, z); 84 | uv = qvec.cross(v); 85 | uuv = qvec.cross(uv); 86 | uv = uv * (2.0f * w); 87 | uuv = uuv * 2.0f; 88 | 89 | return v + uv + uuv; 90 | } 91 | vector3 friend operator* (const vector3& v, const quaternion& q) 92 | { 93 | return q * v; 94 | } 95 | void friend operator*= (vector3& v, const quaternion& q) 96 | { 97 | v = q * v; 98 | } 99 | // 这是是Ogre引擎的实现 (习惯上 父->子 顺序) 100 | quaternion operator*(const quaternion& rkQ) const 101 | { 102 | // NOTE: Multiplication is not generally commutative, so in most 103 | // cases p*q != q*p. 104 | 105 | return quaternion 106 | ( 107 | w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z, 108 | w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y, 109 | w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z, 110 | w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x 111 | ); 112 | } 113 | void operator*= (const quaternion& rkQ) 114 | { 115 | (*this) = (*this) * rkQ; 116 | } 117 | quaternion operator* (real fScalar) const 118 | { 119 | return quaternion(fScalar * w, fScalar * x, fScalar * y, fScalar * z); 120 | } 121 | quaternion friend operator* (real fScalar, const quaternion& rkQ) 122 | { 123 | return quaternion(fScalar * rkQ.w, fScalar * rkQ.x, fScalar * rkQ.y, 124 | fScalar * rkQ.z); 125 | } 126 | 127 | //----------------------------------------------------------------------- 128 | quaternion operator / (real fScalar) const 129 | { 130 | return quaternion(w / fScalar, x / fScalar, y / fScalar, z / fScalar); 131 | } 132 | void operator /= (real fScalar) 133 | { 134 | w /= fScalar; 135 | x /= fScalar; 136 | y /= fScalar; 137 | z /= fScalar; 138 | } 139 | quaternion operator / (const quaternion& q) const 140 | { 141 | return (*this) * q.conjcopy(); 142 | } 143 | vector3 friend operator / (const vector3& v, const quaternion& q) 144 | { 145 | return q.conjcopy() * v; 146 | } 147 | 148 | //----------------------------------------------------------------------- 149 | bool operator == (const quaternion& rkQ) const 150 | { 151 | return w == rkQ.w && x == rkQ.x && y == rkQ.y && z == rkQ.z; 152 | } 153 | //----------------------------------------------------------------------- 154 | real dot(const quaternion& rkQ) const 155 | { 156 | return w * rkQ.w + x * rkQ.x + y * rkQ.y + z * rkQ.z; 157 | } 158 | //----------------------------------------------------------------------- 159 | real length() const 160 | { 161 | return sqrt(w * w + x * x + y * y + z * z); 162 | } 163 | //----------------------------------------------------------------------- 164 | vec3 xyz() const 165 | { 166 | return vec3(x, y, z); 167 | } 168 | vec3 axis() const 169 | { 170 | return vec3(x, y, z).normcopy(); 171 | } 172 | real angle() const 173 | { 174 | if (w >= 1.0) 175 | return 0.0; 176 | if (w <= -1.0) 177 | return PI; 178 | real ang = acos(w) * 2; 179 | if (ang > PI) 180 | return ang - PI * 2; // fixed 181 | if (ang < -PI) 182 | return ang + PI * 2; 183 | return ang; 184 | } 185 | void angle(real ang) 186 | { 187 | ang_axis(ang, axis()); 188 | } 189 | real angle_to(const quaternion& q) const 190 | { 191 | real d = dot(q); 192 | return acos(d * d * 2 - 1); 193 | } 194 | //----------------------------------------------------------------------- 195 | quaternion normalize(void) 196 | { 197 | real len = length(); 198 | if (len != 0) 199 | { 200 | real factor = 1.0f / (len); 201 | *this = *this * factor; 202 | } 203 | return *this; 204 | } 205 | quaternion normalized(void) const 206 | { 207 | real len = length(); 208 | if (len == 0) 209 | { 210 | return quaternion::ONE; 211 | } 212 | return (*this) / len; 213 | } 214 | //----------------------------------------------------------------------- 215 | // 共轭 216 | void conj() 217 | { 218 | this->x = -x; this->y = -y; this->z = -z; 219 | } 220 | quaternion conjcopy() const 221 | { 222 | quaternion q; 223 | q.w = w; 224 | q.x = -x; q.y = -y; q.z = -z; 225 | return q; 226 | } 227 | //----------------------------------------------------------------------- 228 | // 求逆 229 | quaternion inverse() const { 230 | real lenSquared = w * w + x * x + y * y + z * z; 231 | if (lenSquared != 0) { 232 | real factor = 1.0f / lenSquared; 233 | return conjcopy() * factor; 234 | } 235 | return quaternion::ONE; 236 | } 237 | //----------------------------------------------------------------------- 238 | // 指数上运算 239 | quaternion exp() const 240 | { 241 | real r = length(); 242 | vec3 v(x, y, z); 243 | real th = v.len(); 244 | vec3 n = v.normcopy(); 245 | vec3 qv = n * (r * sin(th)); 246 | return quaternion(r * cos(th), qv.x, qv.y, qv.z); 247 | } 248 | friend quaternion exp(const quaternion& q) 249 | { 250 | real r = q.length(); 251 | vec3 v(q.x, q.y, q.z); 252 | real th = v.len(); 253 | vec3 n = v.normcopy(); 254 | vec3 qv = n * (r * sin(th)); 255 | return quaternion(r * cos(th), qv.x, qv.y, qv.z); 256 | } 257 | quaternion log() const { 258 | quaternion src = *this; 259 | vec3 src_v = src.axis() * src.angle(); 260 | return quaternion(src_v.x, src_v.y, src_v.z, 0); 261 | } 262 | // 指数运算（注意运算符的优先级） 263 | quaternion operator ^ (int n) const 264 | { 265 | quaternion ret = *this; 266 | for (int i = 1; i < n; i++) 267 | { 268 | ret = ret * (*this); 269 | } 270 | return ret; 271 | } 272 | quaternion operator ^ (real t) 273 | { 274 | return slerp(t, quaternion::ONE, *this, false); 275 | } 276 | //----------------------------------------------------------------------- 277 | // v1, v2 是单位向量 278 | #define fromvectors from_vectors // 别名 279 | quaternion from_vectors(const vec3& v1, const vec3& v2) 280 | { 281 | real eps = c_tor_dis_level2; // 误差等级 282 | if (fabs(v1.x - v2.x) <= eps && fabs(v1.y - v2.y) <= eps && fabs(v1.z - v2.z) <= eps) 283 | { 284 | (*this) = quaternion::ONE; 285 | } 286 | else 287 | { 288 | real dot = v1.dot(v2); 289 | if (std::abs(dot + 1.0) < eps) // 处理180度的情况 290 | { 291 | vec3 ax; 292 | vec3 uz = vec3::UZ; 293 | if (std::abs(v1.x) < eps && std::abs(v1.y) < eps) 294 | uz = -vec3::UX; 295 | ax = uz.cross(v1).normalized(); 296 | ang_axis(PI, ax.normalized()); 297 | } 298 | else if (dot > -1.0 + eps && dot <= 1.0 - eps) // 处理一般情况 299 | { 300 | vec3 axis = v1.cross(v2).normalized(); 301 | real angle = std::acos(dot); 302 | ang_axis(angle, axis); 303 | } 304 | } 305 | return (*this); 306 | } 307 | //----------------------------------------------------------------------- 308 | // 角度，轴向定义 309 | quaternion ang_axis(real rfAngle, const vec3& rkAxis) 310 | { 311 | // assert: axis[] is unit length 312 | // 313 | // The quaternion representing the rotation is 314 | // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k) 315 | 316 | real fHalfAngle(0.5 * rfAngle); 317 | real fSin = sin(fHalfAngle); 318 | w = cos(fHalfAngle); 319 | x = fSin * rkAxis.x; 320 | y = fSin * rkAxis.y; 321 | z = fSin * rkAxis.z; 322 | 323 | return (*this); 324 | } 325 | //----------------------------------------------------------------------- 326 | #define fromeulers from_eulers // 别名 327 | void from_eulers(real roll, real pitch, real yaw) 328 | { 329 | real t0 = cos(yaw * 0.5); 330 | real t1 = sin(yaw * 0.5); 331 | real t2 = cos(roll * 0.5); 332 | real t3 = sin(roll * 0.5); 333 | real t4 = cos(pitch * 0.5); 334 | real t5 = sin(pitch * 0.5); 335 | 336 | w = t2 * t4 * t0 + t3 * t5 * t1; 337 | x = t3 * t4 * t0 - t2 * t5 * t1; 338 | y = t2 * t5 * t0 + t3 * t4 * t1; 339 | z = t2 * t4 * t1 - t3 * t5 * t0; 340 | } 341 | //----------------------------------------------------------------------- 342 | // roll, pitch, yaw 343 | vec3 to_eulers() const 344 | { 345 | vec3 v; 346 | 347 | real epsilon = 0.00001f; 348 | real halfpi = 0.5 * PI; 349 | 350 | real temp = 2 * (y * z - x * w); 351 | if (temp >= 1 - epsilon) 352 | { 353 | v.x = halfpi; 354 | v.y = -atan2(y, w); 355 | v.z = -atan2(z, w); 356 | } 357 | else if (-temp >= 1 - epsilon) 358 | { 359 | v.x = -halfpi; 360 | v.y = -atan2(y, w); 361 | v.z = -atan2(z, w); 362 | } 363 | else 364 | { 365 | v.x = asin(temp); 366 | v.y = -atan2(x * z + y * w, 0.5 - x * x - y * y); 367 | v.z = -atan2(x * y + z * w, 0.5 - x * x - z * z); 368 | } 369 | return v; 370 | } 371 | void to_eulers(real& roll, real& pitch, real& yaw) const 372 | { 373 | real sinr_cosp = 2 * (w * x + y * z); 374 | real cosr_cosp = 1 - 2 * (x * x + y * y); 375 | roll = atan2(sinr_cosp, cosr_cosp); 376 | 377 | real sinp = 2 * (w * y - z * x); 378 | if (abs(sinp) >= 1) 379 | pitch = copysign(PI / 2, sinp); 380 | else 381 | pitch = asin(sinp); 382 | 383 | real siny_cosp = 2 * (w * z + x * y); 384 | real cosy_cosp = 1 - 2 * (y * y + z * z); 385 | yaw = atan2(siny_cosp, cosy_cosp); 386 | } 387 | //----------------------------------------------------------------------- 388 | static quaternion slerp(const quaternion& qa, const quaternion& qb, real t) { 389 | // quaternion to return 390 | quaternion qm; 391 | // Calculate angle between them. 392 | real cosHalfTheta = qa.w * qb.w + qa.x * qb.x + qa.y * qb.y + qa.z * qb.z; 393 | // if qa=qb or qa=-qb then theta = 0 and we can return qa 394 | if (abs(cosHalfTheta) >= 1.0) { 395 | qm.w = qa.w; qm.x = qa.x; qm.y = qa.y; qm.z = qa.z; 396 | return qm; 397 | } 398 | // Calculate temporary values. 399 | real halfTheta = acos(cosHalfTheta); 400 | real sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta); 401 | // if theta = 180 degrees then result is not fully defined 402 | // we could rotate around any axis normal to qa or qb 403 | if (fabs(sinHalfTheta) < 0.001) { // fabs is floating point absolute 404 | qm.w = (qa.w * 0.5 + qb.w * 0.5); 405 | qm.x = (qa.x * 0.5 + qb.x * 0.5); 406 | qm.y = (qa.y * 0.5 + qb.y * 0.5); 407 | qm.z = (qa.z * 0.5 + qb.z * 0.5); 408 | return qm; 409 | } 410 | real ratioA = sin((1 - t) * halfTheta) / sinHalfTheta; 411 | real ratioB = sin(t * halfTheta) / sinHalfTheta; 412 | //calculate Quaternion. 413 | qm.w = (qa.w * ratioA + qb.w * ratioB); 414 | qm.x = (qa.x * ratioA + qb.x * ratioB); 415 | qm.y = (qa.y * ratioA + qb.y * ratioB); 416 | qm.z = (qa.z * ratioA + qb.z * ratioB); 417 | return qm; 418 | } 419 | //----------------------------------------------------------------------- 420 | static quaternion slerp(real fT, const quaternion& rkP, 421 | const quaternion& rkQ, bool shortestPath) 422 | { 423 | const real msEpsilon = 1e-03; 424 | real fCos = rkP.dot(rkQ); 425 | quaternion rkT; 426 | 427 | // Do we need to invert rotation? 428 | if (fCos < 0.0f && shortestPath) 429 | { 430 | fCos = -fCos; 431 | rkT = -rkQ; 432 | } 433 | else 434 | { 435 | rkT = rkQ; 436 | } 437 | 438 | if (fabs(fCos) < 1 - msEpsilon) 439 | { 440 | // Standard case (slerp) 441 | real fSin = sqrt(1 - (fCos * fCos)); 442 | real fAngle = atan2(fSin, fCos); 443 | real fInvSin = 1.0f / fSin; 444 | real fCoeff0 = sin((1.0f - fT) * fAngle) * fInvSin; 445 | real fCoeff1 = sin(fT * fAngle) * fInvSin; 446 | return fCoeff0 * rkP + fCoeff1 * rkT; 447 | } 448 | else 449 | { 450 | // There are two situations: 451 | // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear 452 | // interpolation safely. 453 | // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there 454 | // are an infinite number of possibilities interpolation. but we haven't 455 | // have method to fix this case, so just use linear interpolation here. 456 | quaternion t = (1.0f - fT) * rkP + fT * rkT; 457 | // taking the complement requires renormalisation 458 | t.normalize(); 459 | return t; 460 | } 461 | } 462 | //----------------------------------------------------------------------- 463 | static quaternion nlerp(real fT, const quaternion& rkP, 464 | const quaternion& rkQ, bool shortestPath) 465 | { 466 | quaternion result; 467 | real fCos = rkP.dot(rkQ); 468 | if (fCos < 0.0f && shortestPath) 469 | { 470 | result = rkP + fT * ((-rkQ) - rkP); 471 | } 472 | else 473 | { 474 | result = rkP + fT * (rkQ - rkP); 475 | } 476 | result.normalize(); 477 | return result; 478 | } 479 | }; 480 | // ********************************************************************** 481 | #if !defined(PM_IMPLEMENTED) 482 | const quaternion quaternion::ONE = quaternion(1, 0, 0, 0); 483 | const quaternion quaternion::UX = quaternion(1, vec3::UX); 484 | const quaternion quaternion::UY = quaternion(1, vec3::UY); 485 | const quaternion quaternion::UZ = quaternion(1, vec3::UZ); 486 | #endif 487 | --------------------------------------------------------------------------------