├── DualQuaternion.cs ├── DualQuaternion.cs.meta ├── DualQuaternionUtils.cs ├── DualQuaternionUtils.cs.meta ├── LICENSE ├── LICENSE.meta ├── QuaternionUtils.cs ├── QuaternionUtils.cs.meta ├── README.md ├── README.md.meta ├── ScrewCoordinates.cs └── ScrewCoordinates.cs.meta /DualQuaternion.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using UnityEngine; 3 | 4 | namespace DQU 5 | { 6 | public struct DualQuaternion 7 | { 8 | // ------------------------------------------------------------------------- 9 | /// @name Factory functions 10 | // ------------------------------------------------------------------------- 11 | 12 | // Generates a dual quaternion with no translation and no rotation either 13 | public static DualQuaternion Identity 14 | { 15 | get 16 | { 17 | return From(Quaternion.identity, Vector3.zero); 18 | } 19 | } 20 | 21 | // Generates a dual quaternion from a given rotation quaternion q and a translation vector t: 22 | public static DualQuaternion From(Quaternion r, Vector3 t) 23 | { 24 | // // Implementation from https://github.com/brainexcerpts/Dual-Quaternion-Skinning-Sample-Codes: 25 | // float w = -0.5f*( t.x * r.x + t.y * r.y + t.z * r.z); 26 | // float i = 0.5f*( t.x * r.w + t.y * r.z - t.z * r.y); 27 | // float j = 0.5f*(-t.x * r.z + t.y * r.w + t.z * r.x); 28 | // float k = 0.5f*( t.x * r.y - t.y * r.x + t.z * r.w); 29 | // return new DualQuaternion(r, new Quaternion{w = w, x = i, y = j, z = k}); 30 | 31 | // Alternative implementation: 32 | var real = r.normalized; 33 | return new DualQuaternion( 34 | real, 35 | (t.ToPureQuaternion() * real).MultipliedWith(0.5f) 36 | ); 37 | } 38 | 39 | // Generates a dual quaternion from a given transformation matrix: 40 | public static DualQuaternion From(Matrix4x4 m) 41 | { 42 | // As proposed by runevision here: https://answers.unity.com/questions/11363/converting-matrix4x4-to-quaternion-vector3.html 43 | 44 | // Adapted from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm 45 | Quaternion q = new Quaternion(); 46 | q.w = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] + m[1, 1] + m[2, 2])) / 2; 47 | q.x = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] - m[1, 1] - m[2, 2])) / 2; 48 | q.y = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] + m[1, 1] - m[2, 2])) / 2; 49 | q.z = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] - m[1, 1] + m[2, 2])) / 2; 50 | q.x *= Mathf.Sign(q.x * (m[2, 1] - m[1, 2])); 51 | q.y *= Mathf.Sign(q.y * (m[0, 2] - m[2, 0])); 52 | q.z *= Mathf.Sign(q.z * (m[1, 0] - m[0, 1])); 53 | 54 | Vector3 t = m.GetColumn(3); 55 | 56 | return From(q, t); 57 | } 58 | 59 | // Calculates the dot product between two DualQuaternions 60 | public static float Dot(DualQuaternion a, DualQuaternion b) 61 | { 62 | return Quaternion.Dot(a.Real, b.Real); 63 | } 64 | 65 | // ------------------------------------------------------------------------- 66 | /// @name Constructors 67 | // ------------------------------------------------------------------------- 68 | 69 | // Fill directly the dual quaternion with two quaternion for the non-dual 70 | // and dual part (in that order): 71 | public DualQuaternion(Quaternion real, Quaternion dual) 72 | { 73 | _quatReal = real; 74 | _quatDual = dual; 75 | } 76 | 77 | // ------------------------------------------------------------------------- 78 | /// @name Methods and Properties 79 | // ------------------------------------------------------------------------- 80 | 81 | // normalize in place: 82 | public void Normalize() 83 | { 84 | var norm = Mathf.Max(_quatReal.Norm(), 1e-6f); 85 | _quatReal = _quatReal.DividedBy(norm); 86 | _quatDual = _quatDual.DividedBy(norm); 87 | } 88 | 89 | // Get a copy of this dual quaternion which is normalized: 90 | public DualQuaternion normalized 91 | { 92 | get 93 | { 94 | DualQuaternion copy = this; 95 | copy.Normalize(); 96 | return copy; 97 | } 98 | } 99 | 100 | public static Vector3 TranslationVectorFrom(Quaternion real, Quaternion dual) 101 | { 102 | // Translation from the normalized dual quaternion equals : 103 | // 2.f * qblend_e * conjugate(qblend_0) 104 | var vr = real.VectorPart(); 105 | var vd = dual.VectorPart(); 106 | Vector3 trans = (vd * real.w - vr * dual.w + Vector3.Cross(vr, vd)) * 2.0f; 107 | return trans; 108 | } 109 | 110 | private static Vector3 Transform(Vector3 p, Quaternion real, Quaternion dual) 111 | { 112 | // Rotate, then translate: 113 | return (real * p) + TranslationVectorFrom(real, dual); 114 | 115 | // // GLM implementation (same results): 116 | // var vr = real.VectorPart(); 117 | // var vd = dual.VectorPart(); 118 | 119 | // return (Vector3.Cross(vr, Vector3.Cross(vr, p) + p * real.w + vd) + vd * real.w - vr * dual.w) * 2.0f + p; 120 | } 121 | 122 | // Transformation of point p with the dual quaternion 123 | public Vector3 Transform(Vector3 p) 124 | { 125 | // As the dual quaternions may be the results from a 126 | // linear blending we have to normalize it: 127 | var nrmlzd = normalized; 128 | return Transform(p, nrmlzd._quatReal, nrmlzd._quatDual); 129 | } 130 | 131 | public Vector3 TransformUnnormalized(Vector3 p) 132 | { 133 | // Who knows what the result of that might be... 134 | return Transform(p, _quatReal, _quatDual); 135 | } 136 | 137 | // Rotate a vector with the dual quaternion 138 | public Vector3 Rotate(Vector3 v) 139 | { 140 | return _quatReal.normalized * v; 141 | } 142 | 143 | public Vector3 TranslationVector 144 | { 145 | get 146 | { 147 | var norm = _quatReal.Norm(); 148 | 149 | // translation vector from dual quaternion part: 150 | Vector3 t; 151 | t.x = 2.0f * (-_quatDual.w * _quatReal.x + _quatDual.x * _quatReal.w - _quatDual.y * _quatReal.z + _quatDual.z * _quatReal.y) / norm; 152 | t.y = 2.0f * (-_quatDual.w * _quatReal.y + _quatDual.x * _quatReal.z + _quatDual.y * _quatReal.w - _quatDual.z * _quatReal.x) / norm; 153 | t.z = 2.0f * (-_quatDual.w * _quatReal.z - _quatDual.x * _quatReal.y + _quatDual.y * _quatReal.x + _quatDual.z * _quatReal.w) / norm; 154 | return t; 155 | } 156 | } 157 | 158 | public Vector3 GetTranslation() 159 | { 160 | Quaternion t = QuaternionUtils.Multiply(QuaternionUtils.MultipliedWith(Dual, 2.0f), Real.Conjugate()); 161 | return new Vector3(t.x, t.y, t.z); 162 | } 163 | 164 | // Convert the dual quaternion to a homogenous matrix 165 | // N.B: Dual quaternion is normalized before conversion 166 | public Matrix4x4 ToMatrix() 167 | { 168 | var norm = _quatReal.Norm(); 169 | var t = TranslationVector; 170 | return Matrix4x4.TRS(t, _quatReal.DividedBy(norm), Vector3.one); 171 | } 172 | 173 | // Returns a new DualQuaternion which is the conjugate of this 174 | public DualQuaternion Conjugate() 175 | { 176 | return new DualQuaternion(Real.Conjugate(), Dual.Conjugate()); 177 | } 178 | 179 | // Returns a new DualQuaternion which is the inverse of this 180 | public DualQuaternion Inverse() 181 | { 182 | // var realInverse = Quaternion.Inverse(Real); 183 | // return new DualQuaternion( 184 | // realInverse, 185 | // realInverse * Dual * realInverse 186 | // ); 187 | 188 | // GLM's implementation of tdualquat::inverse 189 | var realConj = Real.Conjugate(); 190 | var dualConj = Dual.Conjugate(); 191 | return new DualQuaternion( 192 | realConj, 193 | QuaternionUtils.Add(dualConj, realConj.MultipliedWith(-2.0f * Quaternion.Dot(realConj, dualConj))) 194 | ); 195 | } 196 | 197 | // ------------------------------------------------------------------------- 198 | /// @name Operators 199 | // ------------------------------------------------------------------------- 200 | 201 | // Adds two dual quaternions, which means element-wise addition 202 | public static DualQuaternion operator+(DualQuaternion dq1, DualQuaternion dq2) 203 | { 204 | return new DualQuaternion(QuaternionUtils.Add(dq1._quatReal, dq2._quatReal), QuaternionUtils.Add(dq1._quatDual, dq2._quatDual)); 205 | } 206 | 207 | // Multiplies every element of the dual quaternion with the given scalar 208 | public static DualQuaternion operator*(DualQuaternion dq, float scalar) 209 | { 210 | return new DualQuaternion(dq._quatReal.MultipliedWith(scalar), dq._quatDual.MultipliedWith(scalar)); 211 | } 212 | 213 | // Multiplies two dual quaternions from left to right 214 | public static DualQuaternion operator*(DualQuaternion lhs, DualQuaternion rhs) 215 | { 216 | // lhs.Normalize(); 217 | // rhs.Normalize(); 218 | return new DualQuaternion( 219 | QuaternionUtils.Multiply(lhs.Real, rhs.Real), 220 | QuaternionUtils.Add(QuaternionUtils.Multiply(lhs.Real, rhs.Dual), QuaternionUtils.Multiply(lhs.Dual, rhs.Real)) 221 | ); 222 | // return new DualQuaternion( 223 | // QuaternionUtils.Multiply(rhs.Real, lhs.Real), 224 | // QuaternionUtils.Add(QuaternionUtils.Multiply(rhs.Dual, lhs.Real), QuaternionUtils.Multiply(rhs.Real, lhs.Dual)) 225 | // ); 226 | } 227 | 228 | // ------------------------------------------------------------------------- 229 | /// @name Getters and Setters 230 | // ------------------------------------------------------------------------- 231 | 232 | public Quaternion Real 233 | { 234 | get => _quatReal; 235 | set => _quatReal = value; 236 | } 237 | public Quaternion Rotation 238 | { 239 | get => _quatReal; 240 | set => _quatReal = value; 241 | } 242 | 243 | public Quaternion Dual => _quatDual; 244 | public Quaternion Translation => _quatDual; 245 | 246 | // ------------------------------------------------------------------------- 247 | /// @name Attributes 248 | // ------------------------------------------------------------------------- 249 | 250 | // Non-dual part of the dual quaternion. It also represent the rotation. 251 | // @warning If you want to compute the rotation with this don't forget 252 | // to normalize the quaternion as it might be the result of a 253 | // dual quaternion linear blending 254 | // (when overloaded operators like '+' or '*' are used) 255 | private Quaternion _quatReal; 256 | 257 | // Dual part of the dual quaternion which represent the translation. 258 | // translation can be extracted by computing 259 | // 2.f * _quat_e * conjugate(_quat_0) 260 | // @warning don't forget to normalize quat_0 and quat_e : 261 | // quat_0 = quat_0 / || quat_0 || and quat_e = quat_e / || quat_0 || 262 | private Quaternion _quatDual; 263 | } 264 | 265 | } 266 | -------------------------------------------------------------------------------- /DualQuaternion.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: abc3b87a85f20f14f976cb3b4306e33c 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /DualQuaternionUtils.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using UnityEngine; 4 | 5 | namespace DQU 6 | { 7 | // Utils and extension functions for the DualQuaternion class 8 | public static class DualQuaternionUtils 9 | { 10 | public static DualQuaternion BlendWeighted(IList boneTransformations, IList weights) 11 | { 12 | Debug.Assert(boneTransformations.Count > 0); 13 | Debug.Assert(boneTransformations.Count == weights.Count); 14 | var nb_joints = boneTransformations.Count; // Number of joints influencing vertex p 15 | 16 | // Init dual quaternion with first joint transformation: 17 | var dq_blend = boneTransformations[0] * weights[0]; 18 | 19 | var q0 = boneTransformations[0].Rotation; 20 | 21 | // Look up the other joints influencing 'p' if any 22 | for (int j = 1; j < nb_joints; j++) 23 | { 24 | var dq = boneTransformations[j]; 25 | var w = weights[j]; 26 | var dt = Quaternion.Dot(dq.Rotation, q0); 27 | // if (dt <= 0.0f) // TODO: Use Quaternion.kEpsilon maybe? 28 | // { 29 | // w *= -1f; 30 | // } 31 | 32 | var add_dq = dq * w; 33 | dq_blend = dq_blend + add_dq; 34 | } 35 | 36 | return dq_blend; 37 | } 38 | 39 | public static Vector3 TransformPosition(Vector3 p, IList boneTransformations, IList weights) 40 | { 41 | var dq_blend = BlendWeighted(boneTransformations, weights); 42 | 43 | // Compute animated position 44 | return dq_blend.Transform(p); 45 | } 46 | 47 | public static DualQuaternion ToDualQuaternion(this Vector3 p) 48 | { 49 | return DualQuaternion.From(Quaternion.identity, p); 50 | } 51 | 52 | // Returns a new dual quaternion which is computed from the element-wise 53 | // minima of the input dual quaternions. (No idea if this makes any sense ^^) 54 | public static DualQuaternion Min(DualQuaternion a, DualQuaternion b) 55 | { 56 | return new DualQuaternion( 57 | QuaternionUtils.Min(a.Real, b.Real), 58 | QuaternionUtils.Min(a.Dual, b.Dual) 59 | ); 60 | } 61 | 62 | // Returns a new dual quaternion which is computed from the element-wise 63 | // maxima of the input dual quaternions. (No idea if this makes any sense ^^) 64 | public static DualQuaternion Max(DualQuaternion a, DualQuaternion b) 65 | { 66 | return new DualQuaternion( 67 | QuaternionUtils.Max(a.Real, b.Real), 68 | QuaternionUtils.Max(a.Dual, b.Dual) 69 | ); 70 | } 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /DualQuaternionUtils.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 702f070b77f6c11478a10e75a5a3aad8 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Johannes Unterguggenberger 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 | -------------------------------------------------------------------------------- /LICENSE.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 2221e49a68919da498325ae1acd208af 3 | DefaultImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /QuaternionUtils.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using UnityEngine; 3 | 4 | namespace DQU 5 | { 6 | // Utils and extension functions for Unity's Quaternion class 7 | public static class QuaternionUtils 8 | { 9 | // Converts a vector to its quaternion representation, which is a quaternion 10 | // with no rotation, and its vector-components set to the input vector. 11 | public static Quaternion ToPureQuaternion(this Vector3 vec) 12 | { 13 | return new Quaternion(vec.x, vec.y, vec.z, 0f); 14 | } 15 | 16 | // Computes the norm of a Quaternion 17 | public static float Norm(this Quaternion q) 18 | { 19 | return Mathf.Sqrt(q[0] * q[0] + 20 | q[1] * q[1] + 21 | q[2] * q[2] + 22 | q[3] * q[3] ); 23 | } 24 | 25 | // Returns a new quaternion which has the original quaternion's values 26 | // multiplied with the given factor x. 27 | public static Quaternion MultipliedWith(this Quaternion q, float x) 28 | { 29 | q[0] *= x; 30 | q[1] *= x; 31 | q[2] *= x; 32 | q[3] *= x; 33 | return q; 34 | } 35 | 36 | // Performs quaternion multiplication of two given unit-norm quaternions. 37 | // I.e. a single multiplication that is, no sandwiching or so! 38 | public static Quaternion Multiply(Quaternion lhs, Quaternion rhs) 39 | { 40 | // lhs.Normalize(); 41 | // rhs.Normalize(); 42 | var wp = lhs.w * rhs.w - Vector3.Dot(lhs.VectorPart(), rhs.VectorPart()); 43 | var vp = rhs.VectorPart() * lhs.w + lhs.VectorPart() * rhs.w + Vector3.Cross(lhs.VectorPart(), rhs.VectorPart()); 44 | return new Quaternion(vp.x, vp.y, vp.z, wp); 45 | } 46 | 47 | // Returns a new quaternion which has the original quaternion's values 48 | // divided by the given factor x. 49 | public static Quaternion DividedBy(this Quaternion q, float d) 50 | { 51 | var x = 1f / d; 52 | return q.MultipliedWith(x); 53 | } 54 | 55 | // Gets the vector part of the given quaternion 56 | public static Vector3 VectorPart(this Quaternion q) 57 | { 58 | return new Vector3(q.x, q.y, q.z); 59 | } 60 | 61 | // Gets the vector part of the given quaternion, and performs a check if it really is a pure quaternion 62 | public static Vector3 PureQuaternionToPositionVector(this Quaternion q) 63 | { 64 | Debug.Assert(Mathf.Abs(q.w) < 1e-6); 65 | return new Vector3(q.x, q.y, q.z); 66 | } 67 | 68 | 69 | // Perform element-wise addition of both quaternions' values 70 | public static Quaternion Add(Quaternion q1, Quaternion q2) 71 | { 72 | return new Quaternion( 73 | q1.x + q2.x, 74 | q1.y + q2.y, 75 | q1.z + q2.z, 76 | q1.w + q2.w 77 | ); 78 | } 79 | 80 | // Return a new quaternion which is the conjugate of the original 81 | public static Quaternion Conjugate(this Quaternion q) 82 | { 83 | return new Quaternion(-q.x, -q.y, -q.z, q.w); 84 | } 85 | 86 | // Returns a new quaternion which is computed from the element-wise 87 | // minima of the input quaternions. (No idea if this makes any sense ^^) 88 | public static Quaternion Min(Quaternion a, Quaternion b) 89 | { 90 | return new Quaternion( 91 | Mathf.Min(a.x, b.x), 92 | Mathf.Min(a.y, b.y), 93 | Mathf.Min(a.z, b.z), 94 | Mathf.Min(a.w, b.w) 95 | ); 96 | } 97 | 98 | // Returns a new quaternion which is computed from the element-wise 99 | // maxima of the input quaternions. (No idea if this makes any sense ^^) 100 | public static Quaternion Max(Quaternion a, Quaternion b) 101 | { 102 | return new Quaternion( 103 | Mathf.Max(a.x, b.x), 104 | Mathf.Max(a.y, b.y), 105 | Mathf.Max(a.z, b.z), 106 | Mathf.Max(a.w, b.w) 107 | ); 108 | } 109 | } 110 | 111 | } 112 | -------------------------------------------------------------------------------- /QuaternionUtils.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: b317c0344b5643f4bb968c931fb41957 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DualQuaternionsForUnity 2 | C# port of dual quaternion skinning code from brainexcerpts/Dual-Quaternion-Skinning-Sample-Codes 3 | -------------------------------------------------------------------------------- /README.md.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 13dd1ef9a689cfd40892b5f77625d056 3 | TextScriptImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /ScrewCoordinates.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using UnityEngine; 3 | 4 | namespace DQU 5 | { 6 | public struct ScrewCoordinates 7 | { 8 | public static ScrewCoordinates From(DualQuaternion dq) 9 | { 10 | var v_r = new Vector3(dq.Real.x, dq.Real.y, dq.Real.z); 11 | var v_d = new Vector3(dq.Dual.x, dq.Dual.y, dq.Dual.z); 12 | ScrewCoordinates result; 13 | result._p = Vector3.zero; 14 | result._theta = 2f * Mathf.Acos(dq.Real.w); 15 | result._d = -2f * dq.Dual.w / v_r.magnitude; 16 | result._line = v_r.normalized; 17 | result._moment = (v_d - result._line * result._d * dq.Real.w / 2f) / v_r.magnitude; 18 | return result; 19 | } 20 | 21 | // Returns a point on... ? 22 | public Vector3 PointOnTheLine => _p; 23 | 24 | // Returns the angle of rotation around ??? in radians 25 | public float AngleOfRotationRad => _theta; 26 | // Returns the angle of rotation around ??? in degrees 27 | public float AngleOfRotationDeg => _theta * Mathf.Rad2Deg; 28 | 29 | // Returns the translation along the axis 30 | public float TranslationAlongTheAxis => _d; 31 | 32 | public Vector3 PointTranslatedAlongTheAxis => PointOnTheLine + Line * TranslationAlongTheAxis; 33 | 34 | // Returns the line to rotate about, which represent to be the Pluecker Coordiantes 1, 2, and 3. 35 | public Vector3 Line => _line; 36 | 37 | // Returns the moment vector, which happens to represent the Pluecker Coordinates 4, 5, and 6. 38 | public Vector3 MomentVector => _moment; 39 | 40 | public DualQuaternion ToDualQuaternion() 41 | { 42 | var cosHalfTheta = Mathf.Cos(_theta / 2f); 43 | var sinHalfTheta = Mathf.Sin(_theta / 2f); 44 | var halfD = _d / 2f; 45 | 46 | var w_r = cosHalfTheta; 47 | var v_r = _line * sinHalfTheta; 48 | var w_d = -halfD * sinHalfTheta; 49 | var v_d = sinHalfTheta * _moment + halfD * cosHalfTheta * _line; 50 | 51 | return new DualQuaternion(new Quaternion(v_r.x, v_r.y, v_r.z, w_r), new Quaternion(v_d.x, v_d.y, v_d.z, w_d)); 52 | } 53 | 54 | private Vector3 _p; 55 | private float _theta; 56 | private float _d; 57 | private Vector3 _line; 58 | private Vector3 _moment; 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /ScrewCoordinates.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: cac21b8cc59fa4c4dbc43d9128426b35 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | --------------------------------------------------------------------------------