Newton Dynamics  4.00
cuQuat.h
1 /* Copyright (c) <2003-2021> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21 
22 #ifndef __ND_CUQUAT_H__
23 #define __ND_CUQUAT_H__
24 
25 #include "cuVector.h"
26 #include "cuMatrix3x3.h"
27 
28 class cuQuat: public cuVector
29 {
30  public:
31  inline __device__ __host__ cuQuat()
32  :cuVector(0.0f, 0.0f, 0.0f, 1.0f)
33  {
34  }
35 
36  inline __device__ __host__ cuQuat(float x, float y, float z, float w)
37  :cuVector(x, y, z, w)
38  {
39  }
40 
41  inline __device__ __host__ cuQuat(const cuQuat& src)
42  :cuVector(src)
43  {
44  }
45 
46  inline __device__ __host__ cuQuat(const cuVector& src)
47  :cuVector(src)
48  {
49  }
50 
51  inline __device__ __host__ cuQuat(const cuVector& unitAxis, float angle)
52  :cuVector()
53  {
54  angle = angle * 0.5f;
55  w = cos(angle);
56  const float sinAng = sin(angle);
57  x = unitAxis.x * sinAng;
58  y = unitAxis.y * sinAng;
59  z = unitAxis.z * sinAng;
60  }
61 
62  inline cuQuat(const ndVector& src)
63  :cuVector(src)
64  {
65  }
66 
67  inline cuMatrix3x3 __device__ __host__ GetMatrix3x3 () const
68  {
69  //const cuQuat quat0 = *this;
70  const cuQuat quat1 (Scale (2.0));
71 
72  const float x2 = x * quat1.x;
73  const float y2 = y * quat1.y;
74  const float z2 = z * quat1.z;
75 
76  const float xy = x * quat1.y;
77  const float xz = x * quat1.z;
78  const float xw = x * quat1.w;
79  const float yz = y * quat1.z;
80  const float yw = y * quat1.w;
81  const float zw = z * quat1.w;
82 
83  const cuVector front(1.0f - y2 - z2, xy + zw, xz - yw, 0.0f);
84  const cuVector up (xy - zw, 1.0f - x2 - z2, yz + xw, 0.0f);
85  const cuVector right(xz + yw, yz - xw, 1.0f - x2 - y2, 0.0f);
86  return cuMatrix3x3(front, up, right);
87  }
88 
89  inline cuQuat __device__ __host__ Normalize() const
90  {
91  return cuVector::Normalize();
92  }
93 
94  inline cuQuat __device__ __host__ operator* (const cuQuat &q) const
95  {
96  const cuVector x_( q.w, q.z, -q.y, -q.x);
97  const cuVector y_(-q.z, q.w, q.x, -q.y);
98  const cuVector z_( q.y, -q.x, q.w, -q.z);
99  const cuVector w_(q);
100  return x_ * cuVector(x) + y_ * cuVector(y) + z_ * cuVector(z) + w_ * cuVector(w);
101  }
102 };
103 
104 #endif
cuVector
Definition: cuVector.h:32
cuQuat
Definition: cuQuat.h:29
cuMatrix3x3
Definition: cuMatrix3x3.h:29
ndVector
Definition: ndVectorArmNeon.h:41