Debian Codegraph @

src/acl/aclMath/aclQuaternionOfElements.cxx

raw
1 /*
2 * Advanced Simulation Library <http://asl.org.il>
3 *
4 * Copyright 2015 Avtech Scientific <http://avtechscientific.com>
5 *
6 *
7 * This file is part of Advanced Simulation Library (ASL).
8 *
9 * ASL is free software: you can redistribute it and/or modify it
10 * under the terms of the GNU Affero General Public License as
11 * published by the Free Software Foundation, version 3 of the License.
12 *
13 * ASL is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Affero General Public License for more details.
17 *
18 * You should have received a copy of the GNU Affero General Public License
19 * along with ASL. If not, see <http://www.gnu.org/licenses/>.
20 *
21 */
22
23
24 #include "aclQuaternionOfElements.h"
25 #include "aclVectorOfElementsOperations.h"
26 #include "aclGenerators.h"
27 #include "aclElementBase.h"
28
29 using namespace std;
30 using asl::errorMessage;
31
32 namespace acl
33 {
34 QuaternionOfElements::QuaternionOfElements():
35 w(1),
36 u(3)
37 {
38 }
39
40
41 void QuaternionOfElements::setWElement(Element a)
42 {
43 w[0]=a;
44 }
45
46 void QuaternionOfElements::setUElement(unsigned int i, Element a)
47 {
48 u.at(i)=a;
49 }
50
51 const Element QuaternionOfElements::getWElement() const
52 {
53 return w[0];
54 }
55
56 const Element QuaternionOfElements::getUElement(unsigned int i) const
57 {
58 return u.at(i);
59 }
60
61 VectorOfElements & QuaternionOfElements::getW()
62 {
63 return w;
64 }
65
66 VectorOfElements & QuaternionOfElements::getU()
67 {
68 return u;
69 }
70
71 const VectorOfElements & QuaternionOfElements::getW() const
72 {
73 return w;
74 }
75
76 const VectorOfElements & QuaternionOfElements::getU() const
77 {
78 return u;
79 }
80
81
82 void copy(const QuaternionOfElements & source,QuaternionOfElements destination)
83 {
84 copy(source.getW(),destination.getW());
85 copy(source.getU(),destination.getU());
86 }
87
88 QuaternionOfElements operator+(QuaternionOfElements & a,QuaternionOfElements & b)
89 {
90 QuaternionOfElements m;
91 copy(a.getW()+b.getW(),m.getW());
92 copy(a.getU()+b.getU(),m.getU());
93 return m;
94 }
95
96 QuaternionOfElements operator-(QuaternionOfElements & a,QuaternionOfElements & b)
97 {
98 QuaternionOfElements m;
99 copy(a.getW()-b.getW(),m.getW());
100 copy(a.getU()-b.getU(),m.getU());
101 return m;
102 }
103
104 QuaternionOfElements operator*(const QuaternionOfElements & a, const QuaternionOfElements & b)
105 {
106 QuaternionOfElements m;
107 copy(a.getW()*b.getW()-a.getU()*b.getU(),m.getW());
108 copy(a.getW() * b.getU() + b.getW()*a.getU() +
109 crossProduct(a.getU(), b.getU()), m.getU());
110 return m;
111 }
112
113 VectorOfElements l2(QuaternionOfElements & a)
114 {
115 return l2(a.getW())+l2(a.getU());
116 }
117
118 QuaternionOfElements normalize(QuaternionOfElements & a)
119 {
120 VectorOfElements il(generateVEPrivateVariable(1u,a.getW()[0]->getTypeID()));
121 VectorOfElements ileq(1);
122 copy(il=1./sqrt(l2(a)),ileq);
123 QuaternionOfElements m;
124 copy(a.getW()*ileq,m.getW());
125 copy(a.getU()*il,m.getU());
126 return m;
127 }
128
129 } // namespace acl