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
|
#pragma once
#include "schema_math.hpp"
#include <cmath>
namespace saw {
template<typename T>
class data<schema::Quarternion<T>,encode::Native> {
public:
using Schema = schema::Quaternion<T>;
private:
data<schema::FixedArray<T,4u>,encode::Native> vals_;
using NativeType = typename native_data_type<T>::type;
public:
data() = default;
data(data<T,encode::Native> i__, data<T,encode::Native> j__, data<T,encode::Native> k__, data<T,encode::Native> w__):
vals_{i__,j__,k__,w__}
{}
constexpr void normalize() {
data<T,encode::Native> norm{};
for(data<schema::UInt64,encode::Native> i = 0u; i < vals_.size(); ++i){
norm += vals_.at(i) * vals_.at(i);
}
norm.set(std::sqrt(norm.get()));
for(data<schema::UInt64,encode::Native> i = 0u; i < vals_.size(); ++i){
vals_.at(i) = vals_.at(i) / norm;
}
}
constexpr data<T,encode::Native>& at(data<schema::UInt64,encode::Native> i){
return vals_.at(i);
}
constexpr const data<T,encode::Native>& at(data<schema::UInt64,encode::Native> i) const {
return vals_.at(i);
}
constexpr data<Schema, encode::Native> operator*(const data<schema::Quaternion,encode::Native>& rhs) const {
data<Schema,encode::Native> mult;
data<schema::UInt64,encode::Native> i_i{0u};
data<schema::UInt64,encode::Native> j_i{1u};
data<schema::UInt64,encode::Native> k_i{2u};
data<schema::UInt64,encode::Native> w_i{3u};
mult.at(i_i) = at(w_i) * rhs.at(i_i) + at(i_i) * rhs.at(w_i) + at(j_i) * rhs.at(k_i) - at(k_i) * rhs.at(j_i);
mult.at(j_i) = at(w_i) * rhs.at(j_i) + at(j_i) * rhs.at(w_i) - at(i_i) * rhs.at(k_i) + at(k_i) * rhs.at(i_i);
mult.at(k_i) = at(w_i) * rhs.at(k_i) + at(k_i) * rhs.at(w_i) + at(i_i) * rhs.at(j_i) - at(j_i) * rhs.at(i_i);
mult.at(w_i) = at(w_i) * rhs.at(w_i) - at(i_i) * rhs.at(i_i) - at(j_i) * rhs.at(j_i) - at(k_i) * rhs.at(k_i);
return mult;
}
};
}
|
