Dual-Quaternion-Library/DualQuaternion.cpp at master · Hafidzazmi/Dual-Quaternion-Library · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
#include <math.h>
#include <array>
#include "Quaternion.h"
#include "DualQuaternion.h"
using namespace std;

// Create Dual Quaternion
// INPUT  : Angle (Degrees) , Direction of rotation <array 3>, Translation <array 4>
// OUTPUT :  Dual Quaternion <array 8>
array<double, 8> DualQuaternion::Create(double theta, array<int, 3> direction, array<double, 4> translation) {
  array<double, 4> QuaternionScalar, QuaternionVector;
  array<double, 8> Output ;

  QuaternionScalar = Quaternion::Create(theta, direction);
  QuaternionVector = Quaternion::MultiplyScalar(Quaternion::Multiply(translation,QuaternionScalar), 0.5);

  for(int i=0; i <4; i++){
    Output[i]     = QuaternionScalar[i];
    Output[i + 4] = QuaternionVector[i];
  }

  return Output;
}

// Grab Dual Quaternion Pure Rotation
// INPUT  : Dual Quaternion <array 8>
// OUTPUT :  Dual Quaternion <array 8>
array<double, 8> DualQuaternion::Rotation(array<double, 8> p) {
  array<double, 8> Output ;

  for(int i=0; i <4; i++){
    Output[i]     = p[i];
    Output[i + 4] = 0;
  }

  return Output;
}

// Grab Quaternion Translation
// INPUT  : Dual Quaternion <array 8>
// OUTPUT :  Vector Quaternion <array 4>
array<double, 4> DualQuaternion::Translation(array<double, 8> p) {
  array<double, 4> QuaternionScalar, QuaternionVector;
  array<double, 4> Output ;

  for(int i=0; i <4; i++){
    QuaternionScalar[i] = p[i];
    QuaternionVector[i] = p[i + 4];
  }

  Output = Quaternion::MultiplyScalar(Quaternion::Multiply(QuaternionVector, Quaternion::Conjugate(QuaternionScalar)), 2);

  return Output;
}


// Conjugate Quaternion P
// INPUT  : Quaternion <array 4>
// OUTPUT :  Quaternion <array 4>
array<double, 8> DualQuaternion::Conjugate(array<double,8> p){
  array<double, 4> QuaternionScalar, QuaternionVector;
  array<double, 8> Output ;

  for(int i=0; i <4; i++){
    QuaternionScalar[i] = p[i];
    QuaternionVector[i] = p[i + 4];
  }
  QuaternionScalar = Quaternion::Conjugate(QuaternionScalar);
  QuaternionVector = Quaternion::Conjugate(QuaternionVector);

  for(int i=0; i <4; i++){
    Output[i]     = QuaternionScalar[i];
    Output[i + 4] = QuaternionVector[i];
  }

   return Output;
}

// Add Dual Quaternion P + Q
// INPUT  : Dual Quaternion <array 8> , Dual Quaternion <array 8>
// OUTPUT :  Dual Quaternion <array 8>
array<double, 8> DualQuaternion::Add(array<double,8> p, array<double,8> q){
  array<double, 8> Output ;
  for(int i=0; i <8; i++){
    Output[i] = p[i] + q[i];
  }
  return Output;
}


// Subtract Dual Quaternion P - Q
// INPUT  : Dual Quaternion <array 8> , Dual Quaternion <array 8>
// OUTPUT :  Dual Quaternion <array 8>
array<double, 8> DualQuaternion::Subtract(array<double,8> p, array<double,8> q){
  array<double, 8> Output ;
  for(int i=0; i <8; i++){
    Output[i] = p[i] - q[i];
  }
  return Output;
}

// Multiply Dual Quaternion P * Q (matrix form)
// INPUT  : Dual Quaternion <array 8> , Dual Quaternion <array 8>
// OUTPUT : Dual Quaternion <array 8>
array<double, 8> DualQuaternion::Multiply(array<double,8> p, array<double,8> q){
  array<double, 4> QuaternionScalar, QuaternionVector;
  array<double, 4> QuaternionScalarP, QuaternionVectorP;
  array<double, 4> QuaternionScalarQ, QuaternionVectorQ;
  array<double, 8> Output ;

  for(int i=0; i <4; i++){
    QuaternionScalarP[i] = p[i];
    QuaternionVectorP[i] = p[i + 4];
    QuaternionScalarQ[i] = q[i];
    QuaternionVectorQ[i] = q[i + 4];
  }

  QuaternionScalar = Quaternion::Multiply(QuaternionScalarP,QuaternionScalarQ);
  QuaternionVector = Quaternion::Add( Quaternion::Multiply(QuaternionScalarP,QuaternionVectorQ), Quaternion::Multiply(QuaternionVectorP,QuaternionScalarQ) );

  for(int i=0; i <4; i++){
    Output[i]     = QuaternionScalar[i];
    Output[i + 4] = QuaternionVector[i];
  }

  return Output;
}