kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Base: apply clang format

Browse Source
This commit is contained in:
wmayer
2023-11-10 18:27:44 +01:00
committed by WandererFan
parent bb333d9a74
commit 985def3416
154 changed files with 11874 additions and 9872 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
src/Base/DualQuaternion.cpp
View File

@@ -28,79 +28,47 @@
Base::DualQuat Base::operator+(Base::DualQuat a, Base::DualQuat b)
{
return {
a.x + b.x,
a.y + b.y,
a.z + b.z,
a.w + b.w
};
return {a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w};
}
Base::DualQuat Base::operator-(Base::DualQuat a, Base::DualQuat b)
{
return {
a.x - b.x,
a.y - b.y,
a.z - b.z,
a.w - b.w
};
return {a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w};
}
Base::DualQuat Base::operator*(Base::DualQuat a, Base::DualQuat b)
{
return {
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
};
return {a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z};
}
Base::DualQuat Base::operator*(Base::DualQuat a, double b)
{
return {
a.x * b,
a.y * b,
a.z * b,
a.w * b
};
return {a.x * b, a.y * b, a.z * b, a.w * b};
}
Base::DualQuat Base::operator*(double a, Base::DualQuat b)
{
return {
b.x * a,
b.y * a,
b.z * a,
b.w * a
};
return {b.x * a, b.y * a, b.z * a, b.w * a};
}
Base::DualQuat Base::operator*(Base::DualQuat a, Base::DualNumber b)
{
return {
a.x * b,
a.y * b,
a.z * b,
a.w * b
};
return {a.x * b, a.y * b, a.z * b, a.w * b};
}
Base::DualQuat Base::operator*(Base::DualNumber a, Base::DualQuat b)
{
return {
b.x * a,
b.y * a,
b.z * a,
b.w * a
};
return {b.x * a, b.y * a, b.z * a, b.w * a};
}
Base::DualQuat::DualQuat(Base::DualQuat re, Base::DualQuat du)
: x(re.x.re, du.x.re),
y(re.y.re, du.y.re),
z(re.z.re, du.z.re),
w(re.w.re, du.w.re)
: x(re.x.re, du.x.re)
, y(re.y.re, du.y.re)
, z(re.z.re, du.z.re)
, w(re.w.re, du.w.re)
{
assert(re.dual().length() < 1e-12);
assert(du.dual().length() < 1e-12);
@@ -108,10 +76,7 @@ Base::DualQuat::DualQuat(Base::DualQuat re, Base::DualQuat du)
double Base::DualQuat::dot(Base::DualQuat a, Base::DualQuat b)
{
return a.x.re * b.x.re +
a.y.re * b.y.re +
a.z.re * b.z.re +
a.w.re * b.w.re ;
return a.x.re * b.x.re + a.y.re * b.y.re + a.z.re * b.z.re + a.w.re * b.w.re;
}
Base::DualQuat Base::DualQuat::pow(double t, bool shorten) const
@@ -135,32 +100,34 @@ Base::DualQuat Base::DualQuat::pow(double t, bool shorten) const
* */
double le = this->vec().length();
if (le < 1e-12) {
//special case of no rotation. Interpolate position
return {this->real(), this->dual()*t};
// special case of no rotation. Interpolate position
return {this->real(), this->dual() * t};
}
double normmult = 1.0/le;
double normmult = 1.0 / le;
DualQuat self = *this;
if (shorten){
if (dot(self, identity()) < -1e-12){ //using negative tolerance instead of zero, for stability in situations the choice is ambiguous (180-degree rotations)
if (shorten) {
if (dot(self, identity())
< -1e-12) { // using negative tolerance instead of zero, for stability in situations
// the choice is ambiguous (180-degree rotations)
self = -self;
}
}
//to screw coordinates
// to screw coordinates
double theta = self.theta();
double pitch = -2.0 * self.w.du * normmult;
DualQuat l = self.real().vec() * normmult; //abusing DualQuat to store vectors. Very handy in this case.
DualQuat m = (self.dual().vec() - pitch/2*cos(theta/2)*l)*normmult;
DualQuat l = self.real().vec()
* normmult; // abusing DualQuat to store vectors. Very handy in this case.
DualQuat m = (self.dual().vec() - pitch / 2 * cos(theta / 2) * l) * normmult;
//interpolate
// interpolate
theta *= t;
pitch *= t;
//back to quaternion
return {
l * sin(theta/2) + DualQuat(0,0,0,cos(theta/2)),
m * sin(theta/2) + pitch / 2 * cos(theta/2) * l + DualQuat(0,0,0,-pitch/2*sin(theta/2))
};
// back to quaternion
return {l * sin(theta / 2) + DualQuat(0, 0, 0, cos(theta / 2)),
m * sin(theta / 2) + pitch / 2 * cos(theta / 2) * l
+ DualQuat(0, 0, 0, -pitch / 2 * sin(theta / 2))};
}