Wito Wiala
731a37abae
issues with array initialization with functions ([object::default()]), initializing arrays from a function (implemented a simple gpu version using manual MaybeUninit pointers), changing enums with distinct types to structs or changing the selection logic, changing pointer subtraction in light samplers to a scan (this will come back to bite me in the ass), and ignoring the data module, since SPIR-V cant use pointers in statics.
2138 lines
84 KiB
Rust
2138 lines
84 KiB
Rust
use core::iter::{Product, Sum};
|
|
use core::ops::{Add, Div, Index, IndexMut, Mul};
|
|
use num_traits::Float as NumFloat;
|
|
|
|
use super::math::{SquareMatrix, radians, safe_acos};
|
|
use super::quaternion::Quaternion;
|
|
use crate::core::color::{RGB, XYZ};
|
|
use crate::core::geometry::{
|
|
Bounds3f, Frame, Normal, Normal3f, Point, Point3f, Point3fi, Ray, Vector, Vector3f, Vector3fi,
|
|
VectorLike,
|
|
};
|
|
use crate::core::interaction::{
|
|
Interaction, InteractionBase, InteractionTrait, MediumInteraction, SurfaceInteraction,
|
|
};
|
|
use crate::utils::gpu_array_from_fn;
|
|
use crate::{Float, gamma};
|
|
|
|
#[repr(C)]
|
|
#[derive(Debug, Copy, Clone)]
|
|
pub struct TransformGeneric<T: NumFloat> {
|
|
m: SquareMatrix<T, 4>,
|
|
m_inv: SquareMatrix<T, 4>,
|
|
}
|
|
|
|
impl<T: NumFloat + Sum + Product> TransformGeneric<T> {
|
|
pub fn new(m: SquareMatrix<T, 4>, m_inv: SquareMatrix<T, 4>) -> Self {
|
|
Self { m, m_inv }
|
|
}
|
|
|
|
pub fn from_matrix(m: SquareMatrix<T, 4>) -> Option<Self> {
|
|
let inv = m.inverse()?;
|
|
Some(Self { m, m_inv: inv })
|
|
}
|
|
|
|
pub fn from_flat(flat: &[T; 16]) -> Option<Self> {
|
|
let m: SquareMatrix<T, 4> = SquareMatrix::from(flat);
|
|
let inv = m.inverse()?;
|
|
Some(Self { m, m_inv: inv })
|
|
}
|
|
|
|
pub fn identity() -> Self {
|
|
let m: SquareMatrix<T, 4> = SquareMatrix::identity();
|
|
Self { m, m_inv: m }
|
|
}
|
|
|
|
pub fn is_identity(&self) -> bool {
|
|
self.m.is_identity()
|
|
}
|
|
|
|
pub fn inverse(&self) -> Self {
|
|
Self {
|
|
m: self.m_inv,
|
|
m_inv: self.m,
|
|
}
|
|
}
|
|
|
|
pub fn apply_inverse(&self, p: Point<T, 3>) -> Point<T, 3> {
|
|
*self * p
|
|
}
|
|
|
|
pub fn apply_inverse_vector(&self, v: Vector<T, 3>) -> Vector<T, 3> {
|
|
let x = v.x();
|
|
let y = v.y();
|
|
let z = v.z();
|
|
Vector::<T, 3>::new(
|
|
self.m_inv[0][0] * x + self.m_inv[0][1] * y + self.m_inv[0][2] * z,
|
|
self.m_inv[1][0] * x + self.m_inv[1][1] * y + self.m_inv[1][2] * z,
|
|
self.m_inv[2][0] * x + self.m_inv[2][1] * y + self.m_inv[2][2] * z,
|
|
)
|
|
}
|
|
|
|
pub fn apply_inverse_normal(&self, n: Normal<T, 3>) -> Normal<T, 3> {
|
|
*self * n
|
|
}
|
|
|
|
pub fn swaps_handedness(&self) -> bool {
|
|
let s = SquareMatrix::<T, 3>::new([
|
|
[self.m[0][0], self.m[0][1], self.m[0][2]],
|
|
[self.m[1][0], self.m[1][1], self.m[1][2]],
|
|
[self.m[2][0], self.m[2][1], self.m[2][2]],
|
|
]);
|
|
s.determinant() < T::zero()
|
|
}
|
|
|
|
pub fn get_matrix(&self) -> SquareMatrix<T, 4> {
|
|
self.m
|
|
}
|
|
}
|
|
|
|
impl<T: NumFloat + Sum + Product> Default for TransformGeneric<T> {
|
|
fn default() -> Self {
|
|
Self::identity()
|
|
}
|
|
}
|
|
|
|
pub type Transform = TransformGeneric<Float>;
|
|
|
|
impl TransformGeneric<Float> {
|
|
pub fn apply_to_point(&self, p: Point3f) -> Point3f {
|
|
let x = p.x();
|
|
let y = p.y();
|
|
let z = p.z();
|
|
let xp = self.m[0][0] * x + self.m[0][1] * y + self.m[0][2] * z + self.m[0][3];
|
|
let yp = self.m[1][0] * x + self.m[1][1] * y + self.m[1][2] * z + self.m[1][3];
|
|
let zp = self.m[2][0] * x + self.m[2][1] * y + self.m[2][2] * z + self.m[2][3];
|
|
let wp = self.m[3][0] * x + self.m[3][1] * y + self.m[3][2] * z + self.m[3][3];
|
|
|
|
if wp == 1. {
|
|
Point3f::new(xp, yp, zp)
|
|
} else {
|
|
Point3f::new(xp / wp, yp / wp, zp / wp)
|
|
}
|
|
}
|
|
|
|
pub fn apply_to_vector(&self, p: Vector3f) -> Vector3f {
|
|
let x = p.x();
|
|
let y = p.y();
|
|
let z = p.z();
|
|
let xp = self.m[0][0] * x + self.m[0][1] * y + self.m[0][2] * z;
|
|
let yp = self.m[1][0] * x + self.m[1][1] * y + self.m[1][2] * z;
|
|
let zp = self.m[2][0] * x + self.m[2][1] * y + self.m[2][2] * z;
|
|
let wp = self.m[3][0] * x + self.m[3][1] * y + self.m[3][2] * z;
|
|
|
|
if wp == 1. {
|
|
Vector3f::new(xp, yp, zp)
|
|
} else {
|
|
Vector3f::new(xp / wp, yp / wp, zp / wp)
|
|
}
|
|
}
|
|
|
|
pub fn apply_to_normal(&self, p: Normal3f) -> Normal3f {
|
|
let x = p.x();
|
|
let y = p.y();
|
|
let z = p.z();
|
|
let xp = self.m[0][0] * x + self.m[0][1] * y + self.m[0][2] * z;
|
|
let yp = self.m[1][0] * x + self.m[1][1] * y + self.m[1][2] * z;
|
|
let zp = self.m[2][0] * x + self.m[2][1] * y + self.m[2][2] * z;
|
|
let wp = self.m[3][0] * x + self.m[3][1] * y + self.m[3][2] * z;
|
|
|
|
if wp == 1. {
|
|
Normal3f::new(xp, yp, zp)
|
|
} else {
|
|
Normal3f::new(xp / wp, yp / wp, zp / wp)
|
|
}
|
|
}
|
|
|
|
pub fn apply_to_bounds(&self, b: Bounds3f) -> Bounds3f {
|
|
let mut new_bounds = Bounds3f::default();
|
|
for i in 0..8 {
|
|
let corner = self.apply_to_point(b.corner(i));
|
|
new_bounds = new_bounds.union_point(corner);
|
|
}
|
|
new_bounds
|
|
}
|
|
|
|
pub fn apply_to_ray(&self, r: &Ray, t_max: &mut Option<Float>) -> Ray {
|
|
let norm_squared = r.d.norm_squared();
|
|
let mut o = Point3fi::new_from_point(r.o);
|
|
if norm_squared > 0. {
|
|
let dt = r.d.abs().dot(o.error()) / norm_squared;
|
|
let offset = Vector3fi::new_from_vector(r.d * dt);
|
|
o = o + offset;
|
|
|
|
if let Some(t) = t_max.as_mut() {
|
|
*t -= dt;
|
|
}
|
|
}
|
|
Ray::new(o.into(), r.d, Some(r.time), &*r.medium)
|
|
}
|
|
|
|
pub fn apply_to_interval(&self, pi: &Point3fi) -> Point3fi {
|
|
let p = pi.midpoint();
|
|
let x = p.x();
|
|
let y = p.y();
|
|
let z = p.z();
|
|
let xp = self.m[0][0] * x + self.m[0][1] * y + self.m[0][2] * z;
|
|
let yp = self.m[1][0] * x + self.m[1][1] * y + self.m[1][2] * z;
|
|
let zp = self.m[2][0] * x + self.m[2][1] * y + self.m[2][2] * z;
|
|
let wp = self.m[3][0] * x + self.m[3][1] * y + self.m[3][2] * z;
|
|
let mut p_error = Vector3f::default();
|
|
|
|
if pi.is_exact() {
|
|
p_error[0] = gamma(3)
|
|
* ((self.m[0][0] * x).abs()
|
|
+ (self.m[0][1] * y).abs()
|
|
+ (self.m[0][2] + z).abs()
|
|
+ self.m[0][3].abs());
|
|
p_error[1] = gamma(3)
|
|
* ((self.m[1][0] * x).abs()
|
|
+ (self.m[1][1] * y).abs()
|
|
+ (self.m[1][2] + z).abs()
|
|
+ self.m[1][3].abs());
|
|
p_error[2] = gamma(3)
|
|
* ((self.m[2][0] * x).abs()
|
|
+ (self.m[2][1] * y).abs()
|
|
+ (self.m[2][2] + z).abs()
|
|
+ self.m[2][3].abs());
|
|
}
|
|
|
|
if wp == 1. {
|
|
Point3fi::new_with_error(Point([xp, yp, zp]), p_error)
|
|
} else {
|
|
Point3fi::new_with_error(Point([xp / wp, yp / wp, zp / wp]), p_error)
|
|
}
|
|
}
|
|
|
|
pub fn apply_to_vector_interval(&self, vi: &Vector3fi) -> Vector3fi {
|
|
let v = vi.midpoint();
|
|
let x = v.x();
|
|
let y = v.y();
|
|
let z = v.z();
|
|
|
|
// Transform the midpoint of the vector interval
|
|
let xp = self.m[0][0] * x + self.m[0][1] * y + self.m[0][2] * z;
|
|
let yp = self.m[1][0] * x + self.m[1][1] * y + self.m[1][2] * z;
|
|
let zp = self.m[2][0] * x + self.m[2][1] * y + self.m[2][2] * z;
|
|
let wp = self.m[3][0] * x + self.m[3][1] * y + self.m[3][2] * z;
|
|
|
|
let mut v_error = Vector3f::default();
|
|
|
|
// Propagate the error, ignoring the translational part of the matrix
|
|
if vi.is_exact() {
|
|
v_error[0] = gamma(3)
|
|
* ((self.m[0][0] * x).abs() + (self.m[0][1] * y).abs() + (self.m[0][2] * z).abs());
|
|
v_error[1] = gamma(3)
|
|
* ((self.m[1][0] * x).abs() + (self.m[1][1] * y).abs() + (self.m[1][2] * z).abs());
|
|
v_error[2] = gamma(3)
|
|
* ((self.m[2][0] * x).abs() + (self.m[2][1] * y).abs() + (self.m[2][2] * z).abs());
|
|
}
|
|
|
|
if wp == 1. {
|
|
Vector3fi::new_with_error(Vector3f::new(xp, yp, zp), v_error)
|
|
} else {
|
|
Vector3fi::new_with_error(Vector3f::new(xp / wp, yp / wp, zp / wp), v_error)
|
|
}
|
|
}
|
|
|
|
pub fn apply_to_interaction(&self, inter: &Interaction) -> Interaction {
|
|
match inter {
|
|
Interaction::Surface(si) => {
|
|
let mut ret = si.clone();
|
|
|
|
ret.common.pi = self.apply_to_interval(&si.common.pi);
|
|
|
|
let n = self.apply_to_normal(si.common.n);
|
|
ret.common.wo = self.apply_to_vector(si.common.wo).normalize();
|
|
|
|
ret.dpdu = self.apply_to_vector(si.dpdu);
|
|
ret.dpdv = self.apply_to_vector(si.dpdv);
|
|
ret.dndu = self.apply_to_normal(si.dndu);
|
|
ret.dndv = self.apply_to_normal(si.dndv);
|
|
|
|
ret.dpdx = self.apply_to_vector(si.dpdx);
|
|
ret.dpdy = self.apply_to_vector(si.dpdy);
|
|
|
|
let shading_n = self.apply_to_normal(si.shading.n);
|
|
ret.shading.n = shading_n.normalize();
|
|
ret.shading.dpdu = self.apply_to_vector(si.shading.dpdu);
|
|
ret.shading.dpdv = self.apply_to_vector(si.shading.dpdv);
|
|
ret.shading.dndu = self.apply_to_normal(si.shading.dndu);
|
|
ret.shading.dndv = self.apply_to_normal(si.shading.dndv);
|
|
|
|
ret.common.n = n.normalize().face_forward(ret.shading.n);
|
|
|
|
Interaction::Surface(ret)
|
|
}
|
|
|
|
Interaction::Medium(mi) => {
|
|
let mut ret = mi.clone();
|
|
|
|
ret.common.pi = self.apply_to_interval(&mi.common.pi);
|
|
ret.common.n = self.apply_to_normal(mi.common.n).normalize();
|
|
ret.common.wo = self.apply_to_vector(mi.common.wo).normalize();
|
|
|
|
Interaction::Medium(ret)
|
|
}
|
|
|
|
Interaction::Simple(sim) => {
|
|
let mut ret = sim.clone();
|
|
|
|
ret.common.pi = self.apply_to_interval(&sim.common.pi);
|
|
|
|
if sim.common.n != Default::default() {
|
|
ret.common.n = self.apply_to_normal(sim.common.n).normalize();
|
|
}
|
|
if sim.common.wo != Default::default() {
|
|
ret.common.wo = self.apply_to_vector(sim.common.wo).normalize();
|
|
}
|
|
|
|
Interaction::Simple(ret)
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn apply_inverse_interval(&self, p: &Point3fi) -> Point3fi {
|
|
let x = Float::from(p.x());
|
|
let y = Float::from(p.y());
|
|
let z = Float::from(p.z());
|
|
let m_inv = &self.m_inv;
|
|
|
|
// Compute transformed coordinates from point
|
|
let xp = (m_inv[0][0] * x + m_inv[0][1] * y) + (m_inv[0][2] * z + m_inv[0][3]);
|
|
let yp = (m_inv[1][0] * x + m_inv[1][1] * y) + (m_inv[1][2] * z + m_inv[1][3]);
|
|
let zp = (m_inv[2][0] * x + m_inv[2][1] * y) + (m_inv[2][2] * z + m_inv[2][3]);
|
|
let wp = (m_inv[3][0] * x + m_inv[3][1] * y) + (m_inv[3][2] * z + m_inv[3][3]);
|
|
|
|
// Compute absolute error for transformed point
|
|
let g3 = gamma(3);
|
|
|
|
let p_out_error = if p.is_exact() {
|
|
Vector3f::new(
|
|
g3 * (m_inv[0][0] * x).abs() + (m_inv[0][1] * y).abs() + (m_inv[0][2] * z).abs(),
|
|
g3 * (m_inv[1][0] * x).abs() + (m_inv[1][1] * y).abs() + (m_inv[1][2] * z).abs(),
|
|
g3 * (m_inv[2][0] * x).abs() + (m_inv[2][1] * y).abs() + (m_inv[2][2] * z).abs(),
|
|
)
|
|
} else {
|
|
let p_in_error = p.error();
|
|
let g3_plus_1 = g3 + 1.0;
|
|
|
|
Vector3f::new(
|
|
g3_plus_1
|
|
* (m_inv[0][0].abs() * p_in_error.x()
|
|
+ m_inv[0][1].abs() * p_in_error.y()
|
|
+ m_inv[0][2].abs() * p_in_error.z())
|
|
+ g3 * ((m_inv[0][0] * x).abs()
|
|
+ (m_inv[0][1] * y).abs()
|
|
+ (m_inv[0][2] * z).abs()
|
|
+ m_inv[0][3].abs()),
|
|
g3_plus_1
|
|
* (m_inv[1][0].abs() * p_in_error.x()
|
|
+ m_inv[1][1].abs() * p_in_error.y()
|
|
+ m_inv[1][2].abs() * p_in_error.z())
|
|
+ g3 * ((m_inv[1][0] * x).abs()
|
|
+ (m_inv[1][1] * y).abs()
|
|
+ (m_inv[1][2] * z).abs()
|
|
+ m_inv[1][3].abs()),
|
|
g3_plus_1
|
|
* (m_inv[2][0].abs() * p_in_error.x()
|
|
+ m_inv[2][1].abs() * p_in_error.y()
|
|
+ m_inv[2][2].abs() * p_in_error.z())
|
|
+ g3 * ((m_inv[2][0] * x).abs()
|
|
+ (m_inv[2][1] * y).abs()
|
|
+ (m_inv[2][2] * z).abs()
|
|
+ m_inv[2][3].abs()),
|
|
)
|
|
};
|
|
|
|
if wp == 1.0 {
|
|
Point3fi::new_with_error(Point3f::new(xp, yp, zp), p_out_error)
|
|
} else {
|
|
Point3fi::new_with_error(Point3f::new(xp / wp, yp / wp, zp / wp), p_out_error)
|
|
}
|
|
}
|
|
|
|
pub fn apply_inverse_ray(&self, r: &Ray, t_max: Option<Float>) -> (Ray, Float) {
|
|
let mut o = self.apply_inverse_interval(&Point3fi::new_from_point(r.o));
|
|
let d = self.apply_inverse_vector(r.d);
|
|
// Offset ray origin to edge of error bounds and compute _tMax_
|
|
let mut t = 0.;
|
|
let length_squared = d.norm_squared();
|
|
if length_squared > 0. {
|
|
let o_error = Vector3f::new(o.x().width() / 2., o.y().width() / 2., o.z().width() / 2.);
|
|
let dt = d.abs().dot(o_error) / length_squared;
|
|
o = o + Vector3fi::from(d * dt);
|
|
if let Some(t_max) = t_max {
|
|
t = t_max - dt;
|
|
}
|
|
}
|
|
(Ray::new(Point3f::from(o), d, Some(r.time), &*r.medium), t)
|
|
}
|
|
|
|
pub fn to_quaternion(self) -> Quaternion {
|
|
let trace = self.m.trace();
|
|
let mut quat = Quaternion::default();
|
|
if trace > 0. {
|
|
let mut s = (trace + 1.).sqrt();
|
|
quat.w = 0.5 * s;
|
|
s = 0.5 / s;
|
|
|
|
quat.v[0] = (self.m[2][1] - self.m[1][2]) * s;
|
|
quat.v[1] = (self.m[0][2] - self.m[2][0]) * s;
|
|
quat.v[2] = (self.m[1][0] - self.m[0][1]) * s;
|
|
} else {
|
|
let nxt = [1, 2, 0];
|
|
let mut q = [0.; 3];
|
|
let mut i = 0;
|
|
|
|
if self.m[1][1] > self.m[0][0] {
|
|
i = 1;
|
|
}
|
|
if self.m[2][2] > self.m[i][i] {
|
|
i = 2;
|
|
}
|
|
|
|
let j = nxt[i];
|
|
let k = nxt[j];
|
|
let mut s = (self.m[i][i] - (self.m[j][j] + self.m[k][k]) + 1.).sqrt();
|
|
q[i] = 0.5 * s;
|
|
if s != 0. {
|
|
s = 0.5 / s;
|
|
}
|
|
quat.w = (self.m[k][j] - self.m[j][k]) * s;
|
|
q[j] = (self.m[j][i] + self.m[i][j]) * s;
|
|
q[k] = (self.m[k][i] + self.m[i][k]) * s;
|
|
quat.v[0] = q[0];
|
|
quat.v[1] = q[1];
|
|
quat.v[2] = q[2];
|
|
}
|
|
quat
|
|
}
|
|
|
|
pub fn translate(delta: Vector3f) -> Self {
|
|
TransformGeneric {
|
|
m: SquareMatrix::new([
|
|
[1.0, 0.0, 0.0, delta.x()],
|
|
[0.0, 1.0, 0.0, delta.y()],
|
|
[0.0, 0.0, 1.0, delta.z()],
|
|
[0.0, 0.0, 0.0, 1.0],
|
|
]),
|
|
m_inv: SquareMatrix::new([
|
|
[1.0, 0.0, 0.0, -delta.x()],
|
|
[0.0, 1.0, 0.0, -delta.y()],
|
|
[0.0, 0.0, 1.0, -delta.z()],
|
|
[0.0, 0.0, 0.0, 1.0],
|
|
]),
|
|
}
|
|
}
|
|
|
|
pub fn scale(x: Float, y: Float, z: Float) -> Self {
|
|
let m = SquareMatrix::new([
|
|
[x, 0., 0., 0.],
|
|
[0., y, 0., 0.],
|
|
[0., 0., z, 0.],
|
|
[0., 0., 0., 1.],
|
|
]);
|
|
|
|
let m_inv = SquareMatrix::new([
|
|
[1. / x, 0., 0., 0.],
|
|
[0., 1. / y, 0., 0.],
|
|
[0., 0., 1. / z, 0.],
|
|
[0., 0., 0., 1.],
|
|
]);
|
|
Self { m, m_inv }
|
|
}
|
|
|
|
pub fn perspective(fov: Float, n: Float, f: Float) -> Option<TransformGeneric<Float>> {
|
|
let persp: SquareMatrix<Float, 4> = SquareMatrix::new([
|
|
[1., 0., 0., 0.],
|
|
[0., 1., 0., 0.],
|
|
[0., 0., f / (f - n), -f * n / (f - n)],
|
|
[0., 0., 1., 0.],
|
|
]);
|
|
let inv_tan_ang = 1. / (radians(fov) / 2.).tan();
|
|
let persp_transform = TransformGeneric::from_matrix(persp)?;
|
|
Some(TransformGeneric::scale(inv_tan_ang, inv_tan_ang, 1.) * persp_transform)
|
|
}
|
|
|
|
pub fn orthographic(z_near: Float, z_far: Float) -> Self {
|
|
Self::scale(1., 1., 1. / (z_far - z_near)) * Self::translate(Vector3f::new(0., 0., -z_near))
|
|
}
|
|
|
|
pub fn rotate_around_axis(theta: Float, axis: impl Into<Vector3f>) -> Self {
|
|
let sin_theta = theta.to_radians().sin();
|
|
let cos_theta = theta.to_radians().cos();
|
|
TransformGeneric::rotate(sin_theta, cos_theta, axis)
|
|
}
|
|
|
|
pub fn rotate(sin_theta: Float, cos_theta: Float, axis: impl Into<Vector3f>) -> Self {
|
|
let vec_axis: Vector3f = axis.into();
|
|
let a = vec_axis.normalize();
|
|
let mut m: SquareMatrix<Float, 4> = SquareMatrix::default();
|
|
m[0][0] = a.x() * a.x() + (1. - a.x() * a.x()) * cos_theta;
|
|
m[0][1] = a.x() * a.y() * (1. - cos_theta) - a.z() * sin_theta;
|
|
m[0][2] = a.x() * a.z() * (1. - cos_theta) + a.y() * sin_theta;
|
|
m[0][3] = 0.;
|
|
|
|
m[1][0] = a.x() * a.y() * (1. - cos_theta) + a.z() * sin_theta;
|
|
m[1][1] = a.y() * a.y() + (1. - a.y() * a.y()) * cos_theta;
|
|
m[1][2] = a.y() * a.z() * (1. - cos_theta) - a.x() * sin_theta;
|
|
m[1][3] = 0.;
|
|
|
|
m[2][0] = a.x() * a.z() * (1. - cos_theta) - a.y() * sin_theta;
|
|
m[2][1] = a.y() * a.z() * (1. - cos_theta) + a.x() * sin_theta;
|
|
m[2][2] = a.z() * a.z() + (1. - a.z() * a.z()) * cos_theta;
|
|
m[2][3] = 0.;
|
|
TransformGeneric::new(m, m.transpose())
|
|
}
|
|
|
|
pub fn rotate_from_to(from: Vector3f, to: Vector3f) -> Self {
|
|
let refl;
|
|
|
|
if from.x().abs() < 0.72 && to.x().abs() < 0.72 {
|
|
refl = Vector3f::new(1., 0., 0.);
|
|
} else if from.y().abs() < 0.72 && to.y().abs() < 0.72 {
|
|
refl = Vector3f::new(0., 1., 0.);
|
|
} else {
|
|
refl = Vector3f::new(0., 0., 1.);
|
|
}
|
|
|
|
let u = refl - from;
|
|
let v = refl - to;
|
|
let uu = u.dot(u);
|
|
let vv = v.dot(v);
|
|
let uv = u.dot(v);
|
|
let mut r: SquareMatrix<Float, 4> = SquareMatrix::default();
|
|
for i in 0..3 {
|
|
for j in 0..3 {
|
|
let k = if i == j { 1. } else { 0. };
|
|
r[i][j] = k - 2. / uu * u[i] * u[j] - 2. * vv * v[i] * v[j]
|
|
+ 4. * uv / (uu * vv) * v[i] * u[j];
|
|
}
|
|
}
|
|
TransformGeneric::new(r, r.transpose())
|
|
}
|
|
|
|
pub fn rotate_x(theta: Float) -> Self {
|
|
let sin_theta = theta.to_radians().sin();
|
|
let cos_theta = theta.to_radians().cos();
|
|
let m = SquareMatrix::new([
|
|
[1., 0., 0., 0.],
|
|
[0., cos_theta, -sin_theta, 0.],
|
|
[0., sin_theta, cos_theta, 0.],
|
|
[0., 0., 0., 1.],
|
|
]);
|
|
Self {
|
|
m,
|
|
m_inv: m.transpose(),
|
|
}
|
|
}
|
|
|
|
pub fn rotate_y(theta: Float) -> Self {
|
|
let sin_theta = theta.to_radians().sin();
|
|
let cos_theta = theta.to_radians().cos();
|
|
let m = SquareMatrix::new([
|
|
[cos_theta, 0., sin_theta, 0.],
|
|
[0., 1., 0., 0.],
|
|
[-sin_theta, 0., cos_theta, 0.],
|
|
[0., 0., 0., 1.],
|
|
]);
|
|
Self {
|
|
m,
|
|
m_inv: m.transpose(),
|
|
}
|
|
}
|
|
|
|
pub fn rotate_z(theta: Float) -> Self {
|
|
let sin_theta = theta.to_radians().sin();
|
|
let cos_theta = theta.to_radians().cos();
|
|
let m = SquareMatrix::new([
|
|
[cos_theta, -sin_theta, 0., 0.],
|
|
[sin_theta, cos_theta, 0., 0.],
|
|
[0., 0., 1., 0.],
|
|
[0., 0., 0., 1.],
|
|
]);
|
|
Self {
|
|
m,
|
|
m_inv: m.transpose(),
|
|
}
|
|
}
|
|
|
|
pub fn decompose(&self) -> (Vector3f, SquareMatrix<Float, 4>, SquareMatrix<Float, 4>) {
|
|
let t = Vector3f::new(self.m[0][3], self.m[1][3], self.m[2][3]);
|
|
let mut m = self.m;
|
|
for i in 0..3 {
|
|
m[i][3] = 0.;
|
|
m[3][i] = m[i][3];
|
|
}
|
|
|
|
m[3][3] = 1.;
|
|
|
|
let mut norm: Float;
|
|
let mut r = m;
|
|
let mut count = 0;
|
|
loop {
|
|
let rit = r
|
|
.transpose()
|
|
.inverse()
|
|
.expect("Transform is not decomposable");
|
|
let rnext = (r + rit) / 2.;
|
|
norm = 0.0;
|
|
for i in 0..3 {
|
|
let n = (r[i][0] - rnext[i][0]).abs()
|
|
+ (r[i][1] - rnext[i][1]).abs()
|
|
+ (r[i][2] - rnext[i][2]).abs();
|
|
norm = norm.max(n);
|
|
}
|
|
r = rnext;
|
|
count += 1;
|
|
if count > 100 || norm < 0.0001 {
|
|
break;
|
|
}
|
|
}
|
|
let s = r
|
|
.transpose()
|
|
.inverse()
|
|
.expect("Part of decomposition is singular")
|
|
* m;
|
|
(t, r, s)
|
|
}
|
|
|
|
pub fn has_scale(&self, tolerance: Option<Float>) -> bool {
|
|
let la2 = self
|
|
.apply_to_vector(Vector3f::new(1., 0., 0.))
|
|
.norm_squared();
|
|
let lb2 = self
|
|
.apply_to_vector(Vector3f::new(0., 1., 0.))
|
|
.norm_squared();
|
|
let lc2 = self
|
|
.apply_to_vector(Vector3f::new(0., 0., 1.))
|
|
.norm_squared();
|
|
let tol = tolerance.unwrap_or(1e-3);
|
|
(la2 - 1.).abs() > tol || (lb2 - 1.).abs() > tol || (lc2 - 1.).abs() > tol
|
|
}
|
|
}
|
|
|
|
impl<T: num_traits::Float> PartialEq for TransformGeneric<T> {
|
|
fn eq(&self, other: &Self) -> bool {
|
|
self.m == other.m && self.m_inv == other.m_inv
|
|
}
|
|
}
|
|
|
|
impl<T: NumFloat> Mul for TransformGeneric<T> {
|
|
type Output = TransformGeneric<T>;
|
|
|
|
fn mul(self, rhs: TransformGeneric<T>) -> Self::Output {
|
|
TransformGeneric {
|
|
m: self.m * rhs.m,
|
|
m_inv: rhs.m_inv * self.m_inv,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<'b, T> Mul<&'b TransformGeneric<T>> for &TransformGeneric<T>
|
|
where
|
|
T: NumFloat,
|
|
{
|
|
type Output = TransformGeneric<T>;
|
|
fn mul(self, rhs: &'b TransformGeneric<T>) -> Self::Output {
|
|
TransformGeneric {
|
|
m: self.m * rhs.m,
|
|
m_inv: rhs.m_inv * self.m_inv,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Mul<TransformGeneric<Float>> for Float {
|
|
type Output = TransformGeneric<Float>;
|
|
fn mul(self, rhs: TransformGeneric<Float>) -> Self::Output {
|
|
TransformGeneric {
|
|
m: rhs.m * self,
|
|
m_inv: rhs.m_inv * self,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<T> Mul<Point<T, 3>> for TransformGeneric<T>
|
|
where
|
|
T: NumFloat,
|
|
{
|
|
type Output = Point<T, 3>;
|
|
fn mul(self, p: Point<T, 3>) -> Self::Output {
|
|
let xp = self.m[0][0] * p.x() + self.m[0][1] * p.y() + self.m[0][2] * p.z() + self.m[0][3];
|
|
let yp = self.m[1][0] * p.x() + self.m[1][1] * p.y() + self.m[1][2] * p.z() + self.m[1][3];
|
|
let zp = self.m[2][0] * p.x() + self.m[2][1] * p.y() + self.m[2][2] * p.z() + self.m[2][3];
|
|
let wp = self.m[3][0] * p.x() + self.m[3][1] * p.y() + self.m[3][2] * p.z() + self.m[3][3];
|
|
|
|
if wp == T::one() {
|
|
Point([xp, yp, zp])
|
|
} else {
|
|
Point([xp / wp, yp / wp, zp / wp])
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<T> Mul<Vector<T, 3>> for TransformGeneric<T>
|
|
where
|
|
T: NumFloat,
|
|
{
|
|
type Output = Vector<T, 3>;
|
|
fn mul(self, p: Vector<T, 3>) -> Self::Output {
|
|
let xp = self.m[0][0] * p.x() + self.m[0][1] * p.y() + self.m[0][2] * p.z() + self.m[0][3];
|
|
let yp = self.m[1][0] * p.x() + self.m[1][1] * p.y() + self.m[1][2] * p.z() + self.m[1][3];
|
|
let zp = self.m[2][0] * p.x() + self.m[2][1] * p.y() + self.m[2][2] * p.z() + self.m[2][3];
|
|
let wp = self.m[3][0] * p.x() + self.m[3][1] * p.y() + self.m[3][2] * p.z() + self.m[3][3];
|
|
|
|
if wp == T::one() {
|
|
Vector([xp, yp, zp])
|
|
} else {
|
|
Vector([xp / wp, yp / wp, zp / wp])
|
|
}
|
|
}
|
|
}
|
|
|
|
impl<T> Mul<Normal<T, 3>> for TransformGeneric<T>
|
|
where
|
|
T: NumFloat,
|
|
{
|
|
type Output = Normal<T, 3>;
|
|
fn mul(self, p: Normal<T, 3>) -> Self::Output {
|
|
let xp = self.m[0][0] * p.x() + self.m[0][1] * p.y() + self.m[0][2] * p.z() + self.m[0][3];
|
|
let yp = self.m[1][0] * p.x() + self.m[1][1] * p.y() + self.m[1][2] * p.z() + self.m[1][3];
|
|
let zp = self.m[2][0] * p.x() + self.m[2][1] * p.y() + self.m[2][2] * p.z() + self.m[2][3];
|
|
let wp = self.m[3][0] * p.x() + self.m[3][1] * p.y() + self.m[3][2] * p.z() + self.m[3][3];
|
|
|
|
if wp == T::one() {
|
|
Normal([xp, yp, zp])
|
|
} else {
|
|
Normal([xp / wp, yp / wp, zp / wp])
|
|
}
|
|
}
|
|
}
|
|
|
|
impl From<Frame> for TransformGeneric<Float> {
|
|
fn from(frame: Frame) -> Self {
|
|
let values: &[Float; 16] = &[
|
|
frame.x.x(),
|
|
frame.x.y(),
|
|
frame.x.z(),
|
|
0.,
|
|
frame.y.x(),
|
|
frame.y.y(),
|
|
frame.y.z(),
|
|
0.,
|
|
frame.z.x(),
|
|
frame.z.y(),
|
|
frame.z.z(),
|
|
0.,
|
|
0.,
|
|
0.,
|
|
0.,
|
|
1.,
|
|
];
|
|
Self::from_flat(values).expect("Transform must be inversible")
|
|
}
|
|
}
|
|
impl From<Quaternion> for TransformGeneric<Float> {
|
|
fn from(q: Quaternion) -> Self {
|
|
let xx = q.v.x() * q.v.x();
|
|
let yy = q.v.y() * q.v.y();
|
|
let zz = q.v.z() * q.v.z();
|
|
let xy = q.v.x() * q.v.y();
|
|
let xz = q.v.x() * q.v.z();
|
|
let yz = q.v.y() * q.v.z();
|
|
let wx = q.v.x() * q.w;
|
|
let wy = q.v.y() * q.w;
|
|
let wz = q.v.z() * q.w;
|
|
|
|
let mut m = [[0.0; 4]; 4];
|
|
|
|
m[0][0] = 1. - 2. * (yy + zz);
|
|
m[0][1] = 2. * (xy - wz);
|
|
m[0][2] = 2. * (xz + wy);
|
|
|
|
m[1][0] = 2. * (xy + wz);
|
|
m[1][1] = 1. - 2. * (xx + zz);
|
|
m[1][2] = 2. * (yz - wx);
|
|
|
|
m[2][0] = 2. * (xz - wy);
|
|
m[2][1] = 2. * (yz + wx);
|
|
m[2][2] = 1. - 2. * (xx + yy);
|
|
|
|
m[3][3] = 1.;
|
|
|
|
let m_sq = SquareMatrix::new(m);
|
|
// For a pure rotation, the inverse is the transpose.
|
|
let m_inv_sq = m_sq.transpose();
|
|
|
|
TransformGeneric::new(m_sq, m_inv_sq)
|
|
}
|
|
}
|
|
|
|
#[repr(C)]
|
|
#[derive(Default, Debug, Copy, Clone)]
|
|
pub struct DerivativeTerm {
|
|
kc: Float,
|
|
kx: Float,
|
|
ky: Float,
|
|
kz: Float,
|
|
}
|
|
|
|
impl DerivativeTerm {
|
|
pub fn new(kc: Float, kx: Float, ky: Float, kz: Float) -> Self {
|
|
Self { kc, kx, ky, kz }
|
|
}
|
|
|
|
pub fn from_matrix(m: [Float; 4]) -> Self {
|
|
Self {
|
|
kc: m[0],
|
|
kx: m[1],
|
|
ky: m[2],
|
|
kz: m[3],
|
|
}
|
|
}
|
|
}
|
|
|
|
#[repr(C)]
|
|
#[derive(Debug, Copy, Clone)]
|
|
pub struct AnimatedTransform {
|
|
pub start_transform: Transform,
|
|
pub end_transform: Transform,
|
|
pub start_time: Float,
|
|
pub end_time: Float,
|
|
actually_animated: bool,
|
|
t: [Vector3f; 2],
|
|
r: [Quaternion; 2],
|
|
s: [SquareMatrix<Float, 4>; 2],
|
|
has_rotation: bool,
|
|
c1: [DerivativeTerm; 3],
|
|
c2: [DerivativeTerm; 3],
|
|
c3: [DerivativeTerm; 3],
|
|
c4: [DerivativeTerm; 3],
|
|
c5: [DerivativeTerm; 3],
|
|
}
|
|
|
|
impl Default for AnimatedTransform {
|
|
fn default() -> Self {
|
|
Self {
|
|
start_transform: Transform::default(),
|
|
end_transform: Transform::default(),
|
|
start_time: 0.0,
|
|
end_time: 0.0,
|
|
actually_animated: false,
|
|
t: gpu_array_from_fn(|_| Vector3f::default()),
|
|
r: gpu_array_from_fn(|_| Quaternion::default()),
|
|
s: gpu_array_from_fn(|_| SquareMatrix::default()),
|
|
has_rotation: false,
|
|
c1: gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
c2: gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
c3: gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
c4: gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
c5: gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
}
|
|
}
|
|
}
|
|
|
|
impl AnimatedTransform {
|
|
pub fn from_transform(t: &Transform) -> Self {
|
|
Self::new(t, 0., t, 1.)
|
|
}
|
|
|
|
pub fn new(
|
|
start_transform: &Transform,
|
|
start_time: Float,
|
|
end_transform: &Transform,
|
|
end_time: Float,
|
|
) -> Self {
|
|
let actually_animated = start_transform != end_transform;
|
|
|
|
if !actually_animated {
|
|
return Self::default();
|
|
}
|
|
|
|
let (t0, r_temp, s0) = start_transform.decompose();
|
|
let mut rm = r_temp;
|
|
let scale_transform =
|
|
TransformGeneric::from_matrix(rm).expect("Scaling matrix should invertible");
|
|
let r0 = scale_transform.to_quaternion();
|
|
let (t1, r_temp, s1) = end_transform.decompose();
|
|
rm = r_temp;
|
|
|
|
let scale_transform =
|
|
TransformGeneric::from_matrix(rm).expect("Scaling matrix should invertible");
|
|
let mut r1 = scale_transform.to_quaternion();
|
|
|
|
if r0.dot(r1) < 0. {
|
|
r1 = -r1;
|
|
}
|
|
|
|
let has_rotation = r0.dot(r1) < 0.9995;
|
|
let t = [t0, t1];
|
|
let r = [r0, r1];
|
|
let s = [s0, s1];
|
|
|
|
let (c1, c2, c3, c4, c5) = if has_rotation {
|
|
let mut c1: [DerivativeTerm; 3] = gpu_array_from_fn(|_| DerivativeTerm::default());
|
|
let mut c2: [DerivativeTerm; 3] = gpu_array_from_fn(|_| DerivativeTerm::default());
|
|
let mut c3: [DerivativeTerm; 3] = gpu_array_from_fn(|_| DerivativeTerm::default());
|
|
let mut c4: [DerivativeTerm; 3] = gpu_array_from_fn(|_| DerivativeTerm::default());
|
|
let mut c5: [DerivativeTerm; 3] = gpu_array_from_fn(|_| DerivativeTerm::default());
|
|
let cos_theta = r0.dot(r1);
|
|
let theta = safe_acos(cos_theta);
|
|
let qperp: Quaternion = (r0 - r1 * cos_theta).normalize();
|
|
|
|
let t0x = t0.x();
|
|
let t0y = t0.y();
|
|
let t0z = t0.z();
|
|
let t1x = t1.x();
|
|
let t1y = t1.y();
|
|
let t1z = t1.z();
|
|
let q0x = r0.v.x();
|
|
let q0y = r0.v.y();
|
|
let q0z = r0.v.z();
|
|
let q0w = r0.w;
|
|
let qperpx = qperp.v.x();
|
|
let qperpy = qperp.v.y();
|
|
let qperpz = qperp.v.z();
|
|
let qperpw = qperp.w;
|
|
let s000 = s0[0][0];
|
|
let s001 = s0[0][1];
|
|
let s002 = s0[0][2];
|
|
let s010 = s0[1][0];
|
|
let s011 = s0[1][1];
|
|
let s012 = s0[1][2];
|
|
let s020 = s0[2][0];
|
|
let s021 = s0[2][1];
|
|
let s022 = s0[2][2];
|
|
let s100 = s1[0][0];
|
|
let s101 = s1[0][1];
|
|
let s102 = s1[0][2];
|
|
let s110 = s1[1][0];
|
|
let s111 = s1[1][1];
|
|
let s112 = s1[1][2];
|
|
let s120 = s1[2][0];
|
|
let s121 = s1[2][1];
|
|
let s122 = s1[2][2];
|
|
|
|
c1[0] = DerivativeTerm::new(
|
|
-t0x + t1x,
|
|
(-1. + q0y * q0y + q0z * q0z + qperpy * qperpy + qperpz * qperpz) * s000
|
|
+ q0w * q0z * s010
|
|
- qperpx * qperpy * s010
|
|
+ qperpw * qperpz * s010
|
|
- q0w * q0y * s020
|
|
- qperpw * qperpy * s020
|
|
- qperpx * qperpz * s020
|
|
+ s100
|
|
- q0y * q0y * s100
|
|
- q0z * q0z * s100
|
|
- qperpy * qperpy * s100
|
|
- qperpz * qperpz * s100
|
|
- q0w * q0z * s110
|
|
+ qperpx * qperpy * s110
|
|
- qperpw * qperpz * s110
|
|
+ q0w * q0y * s120
|
|
+ qperpw * qperpy * s120
|
|
+ qperpx * qperpz * s120
|
|
+ q0x * (-(q0y * s010) - q0z * s020 + q0y * s110 + q0z * s120),
|
|
(-1. + q0y * q0y + q0z * q0z + qperpy * qperpy + qperpz * qperpz) * s001
|
|
+ q0w * q0z * s011
|
|
- qperpx * qperpy * s011
|
|
+ qperpw * qperpz * s011
|
|
- q0w * q0y * s021
|
|
- qperpw * qperpy * s021
|
|
- qperpx * qperpz * s021
|
|
+ s101
|
|
- q0y * q0y * s101
|
|
- q0z * q0z * s101
|
|
- qperpy * qperpy * s101
|
|
- qperpz * qperpz * s101
|
|
- q0w * q0z * s111
|
|
+ qperpx * qperpy * s111
|
|
- qperpw * qperpz * s111
|
|
+ q0w * q0y * s121
|
|
+ qperpw * qperpy * s121
|
|
+ qperpx * qperpz * s121
|
|
+ q0x * (-(q0y * s011) - q0z * s021 + q0y * s111 + q0z * s121),
|
|
(-1. + q0y * q0y + q0z * q0z + qperpy * qperpy + qperpz * qperpz) * s002
|
|
+ q0w * q0z * s012
|
|
- qperpx * qperpy * s012
|
|
+ qperpw * qperpz * s012
|
|
- q0w * q0y * s022
|
|
- qperpw * qperpy * s022
|
|
- qperpx * qperpz * s022
|
|
+ s102
|
|
- q0y * q0y * s102
|
|
- q0z * q0z * s102
|
|
- qperpy * qperpy * s102
|
|
- qperpz * qperpz * s102
|
|
- q0w * q0z * s112
|
|
+ qperpx * qperpy * s112
|
|
- qperpw * qperpz * s112
|
|
+ q0w * q0y * s122
|
|
+ qperpw * qperpy * s122
|
|
+ qperpx * qperpz * s122
|
|
+ q0x * (-(q0y * s012) - q0z * s022 + q0y * s112 + q0z * s122),
|
|
);
|
|
|
|
c2[0] = DerivativeTerm::new(
|
|
0.,
|
|
-(qperpy * qperpy * s000) - qperpz * qperpz * s000 + qperpx * qperpy * s010
|
|
- qperpw * qperpz * s010
|
|
+ qperpw * qperpy * s020
|
|
+ qperpx * qperpz * s020
|
|
+ q0y * q0y * (s000 - s100)
|
|
+ q0z * q0z * (s000 - s100)
|
|
+ qperpy * qperpy * s100
|
|
+ qperpz * qperpz * s100
|
|
- qperpx * qperpy * s110
|
|
+ qperpw * qperpz * s110
|
|
- qperpw * qperpy * s120
|
|
- qperpx * qperpz * s120
|
|
+ 2. * q0x * qperpy * s010 * theta
|
|
- 2. * q0w * qperpz * s010 * theta
|
|
+ 2. * q0w * qperpy * s020 * theta
|
|
+ 2. * q0x * qperpz * s020 * theta
|
|
+ q0y
|
|
* (q0x * (-s010 + s110)
|
|
+ q0w * (-s020 + s120)
|
|
+ 2. * (-2. * qperpy * s000 + qperpx * s010 + qperpw * s020) * theta)
|
|
+ q0z
|
|
* (q0w * (s010 - s110) + q0x * (-s020 + s120)
|
|
- 2. * (2. * qperpz * s000 + qperpw * s010 - qperpx * s020) * theta),
|
|
-(qperpy * qperpy * s001) - qperpz * qperpz * s001 + qperpx * qperpy * s011
|
|
- qperpw * qperpz * s011
|
|
+ qperpw * qperpy * s021
|
|
+ qperpx * qperpz * s021
|
|
+ q0y * q0y * (s001 - s101)
|
|
+ q0z * q0z * (s001 - s101)
|
|
+ qperpy * qperpy * s101
|
|
+ qperpz * qperpz * s101
|
|
- qperpx * qperpy * s111
|
|
+ qperpw * qperpz * s111
|
|
- qperpw * qperpy * s121
|
|
- qperpx * qperpz * s121
|
|
+ 2. * q0x * qperpy * s011 * theta
|
|
- 2. * q0w * qperpz * s011 * theta
|
|
+ 2. * q0w * qperpy * s021 * theta
|
|
+ 2. * q0x * qperpz * s021 * theta
|
|
+ q0y
|
|
* (q0x * (-s011 + s111)
|
|
+ q0w * (-s021 + s121)
|
|
+ 2. * (-2. * qperpy * s001 + qperpx * s011 + qperpw * s021) * theta)
|
|
+ q0z
|
|
* (q0w * (s011 - s111) + q0x * (-s021 + s121)
|
|
- 2. * (2. * qperpz * s001 + qperpw * s011 - qperpx * s021) * theta),
|
|
-(qperpy * qperpy * s002) - qperpz * qperpz * s002 + qperpx * qperpy * s012
|
|
- qperpw * qperpz * s012
|
|
+ qperpw * qperpy * s022
|
|
+ qperpx * qperpz * s022
|
|
+ q0y * q0y * (s002 - s102)
|
|
+ q0z * q0z * (s002 - s102)
|
|
+ qperpy * qperpy * s102
|
|
+ qperpz * qperpz * s102
|
|
- qperpx * qperpy * s112
|
|
+ qperpw * qperpz * s112
|
|
- qperpw * qperpy * s122
|
|
- qperpx * qperpz * s122
|
|
+ 2. * q0x * qperpy * s012 * theta
|
|
- 2. * q0w * qperpz * s012 * theta
|
|
+ 2. * q0w * qperpy * s022 * theta
|
|
+ 2. * q0x * qperpz * s022 * theta
|
|
+ q0y
|
|
* (q0x * (-s012 + s112)
|
|
+ q0w * (-s022 + s122)
|
|
+ 2. * (-2. * qperpy * s002 + qperpx * s012 + qperpw * s022) * theta)
|
|
+ q0z
|
|
* (q0w * (s012 - s112) + q0x * (-s022 + s122)
|
|
- 2. * (2. * qperpz * s002 + qperpw * s012 - qperpx * s022) * theta),
|
|
);
|
|
|
|
c3[0] = DerivativeTerm::new(
|
|
0.,
|
|
-2. * (q0x * qperpy * s010 - q0w * qperpz * s010
|
|
+ q0w * qperpy * s020
|
|
+ q0x * qperpz * s020
|
|
- q0x * qperpy * s110
|
|
+ q0w * qperpz * s110
|
|
- q0w * qperpy * s120
|
|
- q0x * qperpz * s120
|
|
+ q0y
|
|
* (-2. * qperpy * s000
|
|
+ qperpx * s010
|
|
+ qperpw * s020
|
|
+ 2. * qperpy * s100
|
|
- qperpx * s110
|
|
- qperpw * s120)
|
|
+ q0z
|
|
* (-2. * qperpz * s000 - qperpw * s010
|
|
+ qperpx * s020
|
|
+ 2. * qperpz * s100
|
|
+ qperpw * s110
|
|
- qperpx * s120))
|
|
* theta,
|
|
-2. * (q0x * qperpy * s011 - q0w * qperpz * s011
|
|
+ q0w * qperpy * s021
|
|
+ q0x * qperpz * s021
|
|
- q0x * qperpy * s111
|
|
+ q0w * qperpz * s111
|
|
- q0w * qperpy * s121
|
|
- q0x * qperpz * s121
|
|
+ q0y
|
|
* (-2. * qperpy * s001
|
|
+ qperpx * s011
|
|
+ qperpw * s021
|
|
+ 2. * qperpy * s101
|
|
- qperpx * s111
|
|
- qperpw * s121)
|
|
+ q0z
|
|
* (-2. * qperpz * s001 - qperpw * s011
|
|
+ qperpx * s021
|
|
+ 2. * qperpz * s101
|
|
+ qperpw * s111
|
|
- qperpx * s121))
|
|
* theta,
|
|
-2. * (q0x * qperpy * s012 - q0w * qperpz * s012
|
|
+ q0w * qperpy * s022
|
|
+ q0x * qperpz * s022
|
|
- q0x * qperpy * s112
|
|
+ q0w * qperpz * s112
|
|
- q0w * qperpy * s122
|
|
- q0x * qperpz * s122
|
|
+ q0y
|
|
* (-2. * qperpy * s002
|
|
+ qperpx * s012
|
|
+ qperpw * s022
|
|
+ 2. * qperpy * s102
|
|
- qperpx * s112
|
|
- qperpw * s122)
|
|
+ q0z
|
|
* (-2. * qperpz * s002 - qperpw * s012
|
|
+ qperpx * s022
|
|
+ 2. * qperpz * s102
|
|
+ qperpw * s112
|
|
- qperpx * s122))
|
|
* theta,
|
|
);
|
|
|
|
c4[0] = DerivativeTerm::new(
|
|
0.,
|
|
-(q0x * qperpy * s010) + q0w * qperpz * s010
|
|
- q0w * qperpy * s020
|
|
- q0x * qperpz * s020
|
|
+ q0x * qperpy * s110
|
|
- q0w * qperpz * s110
|
|
+ q0w * qperpy * s120
|
|
+ q0x * qperpz * s120
|
|
+ 2. * q0y * q0y * s000 * theta
|
|
+ 2. * q0z * q0z * s000 * theta
|
|
- 2. * qperpy * qperpy * s000 * theta
|
|
- 2. * qperpz * qperpz * s000 * theta
|
|
+ 2. * qperpx * qperpy * s010 * theta
|
|
- 2. * qperpw * qperpz * s010 * theta
|
|
+ 2. * qperpw * qperpy * s020 * theta
|
|
+ 2. * qperpx * qperpz * s020 * theta
|
|
+ q0y
|
|
* (-(qperpx * s010) - qperpw * s020
|
|
+ 2. * qperpy * (s000 - s100)
|
|
+ qperpx * s110
|
|
+ qperpw * s120
|
|
- 2. * q0x * s010 * theta
|
|
- 2. * q0w * s020 * theta)
|
|
+ q0z
|
|
* (2. * qperpz * s000 + qperpw * s010
|
|
- qperpx * s020
|
|
- 2. * qperpz * s100
|
|
- qperpw * s110
|
|
+ qperpx * s120
|
|
+ 2. * q0w * s010 * theta
|
|
- 2. * q0x * s020 * theta),
|
|
-(q0x * qperpy * s011) + q0w * qperpz * s011
|
|
- q0w * qperpy * s021
|
|
- q0x * qperpz * s021
|
|
+ q0x * qperpy * s111
|
|
- q0w * qperpz * s111
|
|
+ q0w * qperpy * s121
|
|
+ q0x * qperpz * s121
|
|
+ 2. * q0y * q0y * s001 * theta
|
|
+ 2. * q0z * q0z * s001 * theta
|
|
- 2. * qperpy * qperpy * s001 * theta
|
|
- 2. * qperpz * qperpz * s001 * theta
|
|
+ 2. * qperpx * qperpy * s011 * theta
|
|
- 2. * qperpw * qperpz * s011 * theta
|
|
+ 2. * qperpw * qperpy * s021 * theta
|
|
+ 2. * qperpx * qperpz * s021 * theta
|
|
+ q0y
|
|
* (-(qperpx * s011) - qperpw * s021
|
|
+ 2. * qperpy * (s001 - s101)
|
|
+ qperpx * s111
|
|
+ qperpw * s121
|
|
- 2. * q0x * s011 * theta
|
|
- 2. * q0w * s021 * theta)
|
|
+ q0z
|
|
* (2. * qperpz * s001 + qperpw * s011
|
|
- qperpx * s021
|
|
- 2. * qperpz * s101
|
|
- qperpw * s111
|
|
+ qperpx * s121
|
|
+ 2. * q0w * s011 * theta
|
|
- 2. * q0x * s021 * theta),
|
|
-(q0x * qperpy * s012) + q0w * qperpz * s012
|
|
- q0w * qperpy * s022
|
|
- q0x * qperpz * s022
|
|
+ q0x * qperpy * s112
|
|
- q0w * qperpz * s112
|
|
+ q0w * qperpy * s122
|
|
+ q0x * qperpz * s122
|
|
+ 2. * q0y * q0y * s002 * theta
|
|
+ 2. * q0z * q0z * s002 * theta
|
|
- 2. * qperpy * qperpy * s002 * theta
|
|
- 2. * qperpz * qperpz * s002 * theta
|
|
+ 2. * qperpx * qperpy * s012 * theta
|
|
- 2. * qperpw * qperpz * s012 * theta
|
|
+ 2. * qperpw * qperpy * s022 * theta
|
|
+ 2. * qperpx * qperpz * s022 * theta
|
|
+ q0y
|
|
* (-(qperpx * s012) - qperpw * s022
|
|
+ 2. * qperpy * (s002 - s102)
|
|
+ qperpx * s112
|
|
+ qperpw * s122
|
|
- 2. * q0x * s012 * theta
|
|
- 2. * q0w * s022 * theta)
|
|
+ q0z
|
|
* (2. * qperpz * s002 + qperpw * s012
|
|
- qperpx * s022
|
|
- 2. * qperpz * s102
|
|
- qperpw * s112
|
|
+ qperpx * s122
|
|
+ 2. * q0w * s012 * theta
|
|
- 2. * q0x * s022 * theta),
|
|
);
|
|
|
|
c5[0] = DerivativeTerm::new(
|
|
0.,
|
|
2. * (qperpy * qperpy * s000 + qperpz * qperpz * s000 - qperpx * qperpy * s010
|
|
+ qperpw * qperpz * s010
|
|
- qperpw * qperpy * s020
|
|
- qperpx * qperpz * s020
|
|
- qperpy * qperpy * s100
|
|
- qperpz * qperpz * s100
|
|
+ q0y * q0y * (-s000 + s100)
|
|
+ q0z * q0z * (-s000 + s100)
|
|
+ qperpx * qperpy * s110
|
|
- qperpw * qperpz * s110
|
|
+ q0y * (q0x * (s010 - s110) + q0w * (s020 - s120))
|
|
+ qperpw * qperpy * s120
|
|
+ qperpx * qperpz * s120
|
|
+ q0z * (-(q0w * s010) + q0x * s020 + q0w * s110 - q0x * s120))
|
|
* theta,
|
|
2. * (qperpy * qperpy * s001 + qperpz * qperpz * s001 - qperpx * qperpy * s011
|
|
+ qperpw * qperpz * s011
|
|
- qperpw * qperpy * s021
|
|
- qperpx * qperpz * s021
|
|
- qperpy * qperpy * s101
|
|
- qperpz * qperpz * s101
|
|
+ q0y * q0y * (-s001 + s101)
|
|
+ q0z * q0z * (-s001 + s101)
|
|
+ qperpx * qperpy * s111
|
|
- qperpw * qperpz * s111
|
|
+ q0y * (q0x * (s011 - s111) + q0w * (s021 - s121))
|
|
+ qperpw * qperpy * s121
|
|
+ qperpx * qperpz * s121
|
|
+ q0z * (-(q0w * s011) + q0x * s021 + q0w * s111 - q0x * s121))
|
|
* theta,
|
|
2. * (qperpy * qperpy * s002 + qperpz * qperpz * s002 - qperpx * qperpy * s012
|
|
+ qperpw * qperpz * s012
|
|
- qperpw * qperpy * s022
|
|
- qperpx * qperpz * s022
|
|
- qperpy * qperpy * s102
|
|
- qperpz * qperpz * s102
|
|
+ q0y * q0y * (-s002 + s102)
|
|
+ q0z * q0z * (-s002 + s102)
|
|
+ qperpx * qperpy * s112
|
|
- qperpw * qperpz * s112
|
|
+ q0y * (q0x * (s012 - s112) + q0w * (s022 - s122))
|
|
+ qperpw * qperpy * s122
|
|
+ qperpx * qperpz * s122
|
|
+ q0z * (-(q0w * s012) + q0x * s022 + q0w * s112 - q0x * s122))
|
|
* theta,
|
|
);
|
|
|
|
c1[1] = DerivativeTerm::new(
|
|
-t0y + t1y,
|
|
-(qperpx * qperpy * s000) - qperpw * qperpz * s000 - s010
|
|
+ q0z * q0z * s010
|
|
+ qperpx * qperpx * s010
|
|
+ qperpz * qperpz * s010
|
|
- q0y * q0z * s020
|
|
+ qperpw * qperpx * s020
|
|
- qperpy * qperpz * s020
|
|
+ qperpx * qperpy * s100
|
|
+ qperpw * qperpz * s100
|
|
+ q0w * q0z * (-s000 + s100)
|
|
+ q0x * q0x * (s010 - s110)
|
|
+ s110
|
|
- q0z * q0z * s110
|
|
- qperpx * qperpx * s110
|
|
- qperpz * qperpz * s110
|
|
+ q0x * (q0y * (-s000 + s100) + q0w * (s020 - s120))
|
|
+ q0y * q0z * s120
|
|
- qperpw * qperpx * s120
|
|
+ qperpy * qperpz * s120,
|
|
-(qperpx * qperpy * s001) - qperpw * qperpz * s001 - s011
|
|
+ q0z * q0z * s011
|
|
+ qperpx * qperpx * s011
|
|
+ qperpz * qperpz * s011
|
|
- q0y * q0z * s021
|
|
+ qperpw * qperpx * s021
|
|
- qperpy * qperpz * s021
|
|
+ qperpx * qperpy * s101
|
|
+ qperpw * qperpz * s101
|
|
+ q0w * q0z * (-s001 + s101)
|
|
+ q0x * q0x * (s011 - s111)
|
|
+ s111
|
|
- q0z * q0z * s111
|
|
- qperpx * qperpx * s111
|
|
- qperpz * qperpz * s111
|
|
+ q0x * (q0y * (-s001 + s101) + q0w * (s021 - s121))
|
|
+ q0y * q0z * s121
|
|
- qperpw * qperpx * s121
|
|
+ qperpy * qperpz * s121,
|
|
-(qperpx * qperpy * s002) - qperpw * qperpz * s002 - s012
|
|
+ q0z * q0z * s012
|
|
+ qperpx * qperpx * s012
|
|
+ qperpz * qperpz * s012
|
|
- q0y * q0z * s022
|
|
+ qperpw * qperpx * s022
|
|
- qperpy * qperpz * s022
|
|
+ qperpx * qperpy * s102
|
|
+ qperpw * qperpz * s102
|
|
+ q0w * q0z * (-s002 + s102)
|
|
+ q0x * q0x * (s012 - s112)
|
|
+ s112
|
|
- q0z * q0z * s112
|
|
- qperpx * qperpx * s112
|
|
- qperpz * qperpz * s112
|
|
+ q0x * (q0y * (-s002 + s102) + q0w * (s022 - s122))
|
|
+ q0y * q0z * s122
|
|
- qperpw * qperpx * s122
|
|
+ qperpy * qperpz * s122,
|
|
);
|
|
|
|
c2[1] = DerivativeTerm::new(
|
|
0.,
|
|
qperpx * qperpy * s000 + qperpw * qperpz * s000 + q0z * q0z * s010
|
|
- qperpx * qperpx * s010
|
|
- qperpz * qperpz * s010
|
|
- q0y * q0z * s020
|
|
- qperpw * qperpx * s020
|
|
+ qperpy * qperpz * s020
|
|
- qperpx * qperpy * s100
|
|
- qperpw * qperpz * s100
|
|
+ q0x * q0x * (s010 - s110)
|
|
- q0z * q0z * s110
|
|
+ qperpx * qperpx * s110
|
|
+ qperpz * qperpz * s110
|
|
+ q0y * q0z * s120
|
|
+ qperpw * qperpx * s120
|
|
- qperpy * qperpz * s120
|
|
+ 2. * q0z * qperpw * s000 * theta
|
|
+ 2. * q0y * qperpx * s000 * theta
|
|
- 4. * q0z * qperpz * s010 * theta
|
|
+ 2. * q0z * qperpy * s020 * theta
|
|
+ 2. * q0y * qperpz * s020 * theta
|
|
+ q0x
|
|
* (q0w * s020 + q0y * (-s000 + s100) - q0w * s120
|
|
+ 2. * qperpy * s000 * theta
|
|
- 4. * qperpx * s010 * theta
|
|
- 2. * qperpw * s020 * theta)
|
|
+ q0w
|
|
* (-(q0z * s000) + q0z * s100 + 2. * qperpz * s000 * theta
|
|
- 2. * qperpx * s020 * theta),
|
|
qperpx * qperpy * s001 + qperpw * qperpz * s001 + q0z * q0z * s011
|
|
- qperpx * qperpx * s011
|
|
- qperpz * qperpz * s011
|
|
- q0y * q0z * s021
|
|
- qperpw * qperpx * s021
|
|
+ qperpy * qperpz * s021
|
|
- qperpx * qperpy * s101
|
|
- qperpw * qperpz * s101
|
|
+ q0x * q0x * (s011 - s111)
|
|
- q0z * q0z * s111
|
|
+ qperpx * qperpx * s111
|
|
+ qperpz * qperpz * s111
|
|
+ q0y * q0z * s121
|
|
+ qperpw * qperpx * s121
|
|
- qperpy * qperpz * s121
|
|
+ 2. * q0z * qperpw * s001 * theta
|
|
+ 2. * q0y * qperpx * s001 * theta
|
|
- 4. * q0z * qperpz * s011 * theta
|
|
+ 2. * q0z * qperpy * s021 * theta
|
|
+ 2. * q0y * qperpz * s021 * theta
|
|
+ q0x
|
|
* (q0w * s021 + q0y * (-s001 + s101) - q0w * s121
|
|
+ 2. * qperpy * s001 * theta
|
|
- 4. * qperpx * s011 * theta
|
|
- 2. * qperpw * s021 * theta)
|
|
+ q0w
|
|
* (-(q0z * s001) + q0z * s101 + 2. * qperpz * s001 * theta
|
|
- 2. * qperpx * s021 * theta),
|
|
qperpx * qperpy * s002 + qperpw * qperpz * s002 + q0z * q0z * s012
|
|
- qperpx * qperpx * s012
|
|
- qperpz * qperpz * s012
|
|
- q0y * q0z * s022
|
|
- qperpw * qperpx * s022
|
|
+ qperpy * qperpz * s022
|
|
- qperpx * qperpy * s102
|
|
- qperpw * qperpz * s102
|
|
+ q0x * q0x * (s012 - s112)
|
|
- q0z * q0z * s112
|
|
+ qperpx * qperpx * s112
|
|
+ qperpz * qperpz * s112
|
|
+ q0y * q0z * s122
|
|
+ qperpw * qperpx * s122
|
|
- qperpy * qperpz * s122
|
|
+ 2. * q0z * qperpw * s002 * theta
|
|
+ 2. * q0y * qperpx * s002 * theta
|
|
- 4. * q0z * qperpz * s012 * theta
|
|
+ 2. * q0z * qperpy * s022 * theta
|
|
+ 2. * q0y * qperpz * s022 * theta
|
|
+ q0x
|
|
* (q0w * s022 + q0y * (-s002 + s102) - q0w * s122
|
|
+ 2. * qperpy * s002 * theta
|
|
- 4. * qperpx * s012 * theta
|
|
- 2. * qperpw * s022 * theta)
|
|
+ q0w
|
|
* (-(q0z * s002) + q0z * s102 + 2. * qperpz * s002 * theta
|
|
- 2. * qperpx * s022 * theta),
|
|
);
|
|
|
|
c3[1] = DerivativeTerm::new(
|
|
0.,
|
|
2. * (-(q0x * qperpy * s000) - q0w * qperpz * s000
|
|
+ 2. * q0x * qperpx * s010
|
|
+ q0x * qperpw * s020
|
|
+ q0w * qperpx * s020
|
|
+ q0x * qperpy * s100
|
|
+ q0w * qperpz * s100
|
|
- 2. * q0x * qperpx * s110
|
|
- q0x * qperpw * s120
|
|
- q0w * qperpx * s120
|
|
+ q0z
|
|
* (2. * qperpz * s010 - qperpy * s020 + qperpw * (-s000 + s100)
|
|
- 2. * qperpz * s110
|
|
+ qperpy * s120)
|
|
+ q0y * (-(qperpx * s000) - qperpz * s020 + qperpx * s100 + qperpz * s120))
|
|
* theta,
|
|
2. * (-(q0x * qperpy * s001) - q0w * qperpz * s001
|
|
+ 2. * q0x * qperpx * s011
|
|
+ q0x * qperpw * s021
|
|
+ q0w * qperpx * s021
|
|
+ q0x * qperpy * s101
|
|
+ q0w * qperpz * s101
|
|
- 2. * q0x * qperpx * s111
|
|
- q0x * qperpw * s121
|
|
- q0w * qperpx * s121
|
|
+ q0z
|
|
* (2. * qperpz * s011 - qperpy * s021 + qperpw * (-s001 + s101)
|
|
- 2. * qperpz * s111
|
|
+ qperpy * s121)
|
|
+ q0y * (-(qperpx * s001) - qperpz * s021 + qperpx * s101 + qperpz * s121))
|
|
* theta,
|
|
2. * (-(q0x * qperpy * s002) - q0w * qperpz * s002
|
|
+ 2. * q0x * qperpx * s012
|
|
+ q0x * qperpw * s022
|
|
+ q0w * qperpx * s022
|
|
+ q0x * qperpy * s102
|
|
+ q0w * qperpz * s102
|
|
- 2. * q0x * qperpx * s112
|
|
- q0x * qperpw * s122
|
|
- q0w * qperpx * s122
|
|
+ q0z
|
|
* (2. * qperpz * s012 - qperpy * s022 + qperpw * (-s002 + s102)
|
|
- 2. * qperpz * s112
|
|
+ qperpy * s122)
|
|
+ q0y * (-(qperpx * s002) - qperpz * s022 + qperpx * s102 + qperpz * s122))
|
|
* theta,
|
|
);
|
|
|
|
c4[1] = DerivativeTerm::new(
|
|
0.,
|
|
-(q0x * qperpy * s000) - q0w * qperpz * s000
|
|
+ 2. * q0x * qperpx * s010
|
|
+ q0x * qperpw * s020
|
|
+ q0w * qperpx * s020
|
|
+ q0x * qperpy * s100
|
|
+ q0w * qperpz * s100
|
|
- 2. * q0x * qperpx * s110
|
|
- q0x * qperpw * s120
|
|
- q0w * qperpx * s120
|
|
+ 2. * qperpx * qperpy * s000 * theta
|
|
+ 2. * qperpw * qperpz * s000 * theta
|
|
+ 2. * q0x * q0x * s010 * theta
|
|
+ 2. * q0z * q0z * s010 * theta
|
|
- 2. * qperpx * qperpx * s010 * theta
|
|
- 2. * qperpz * qperpz * s010 * theta
|
|
+ 2. * q0w * q0x * s020 * theta
|
|
- 2. * qperpw * qperpx * s020 * theta
|
|
+ 2. * qperpy * qperpz * s020 * theta
|
|
+ q0y
|
|
* (-(qperpx * s000) - qperpz * s020 + qperpx * s100 + qperpz * s120
|
|
- 2. * q0x * s000 * theta)
|
|
+ q0z
|
|
* (2. * qperpz * s010 - qperpy * s020 + qperpw * (-s000 + s100)
|
|
- 2. * qperpz * s110
|
|
+ qperpy * s120
|
|
- 2. * q0w * s000 * theta
|
|
- 2. * q0y * s020 * theta),
|
|
-(q0x * qperpy * s001) - q0w * qperpz * s001
|
|
+ 2. * q0x * qperpx * s011
|
|
+ q0x * qperpw * s021
|
|
+ q0w * qperpx * s021
|
|
+ q0x * qperpy * s101
|
|
+ q0w * qperpz * s101
|
|
- 2. * q0x * qperpx * s111
|
|
- q0x * qperpw * s121
|
|
- q0w * qperpx * s121
|
|
+ 2. * qperpx * qperpy * s001 * theta
|
|
+ 2. * qperpw * qperpz * s001 * theta
|
|
+ 2. * q0x * q0x * s011 * theta
|
|
+ 2. * q0z * q0z * s011 * theta
|
|
- 2. * qperpx * qperpx * s011 * theta
|
|
- 2. * qperpz * qperpz * s011 * theta
|
|
+ 2. * q0w * q0x * s021 * theta
|
|
- 2. * qperpw * qperpx * s021 * theta
|
|
+ 2. * qperpy * qperpz * s021 * theta
|
|
+ q0y
|
|
* (-(qperpx * s001) - qperpz * s021 + qperpx * s101 + qperpz * s121
|
|
- 2. * q0x * s001 * theta)
|
|
+ q0z
|
|
* (2. * qperpz * s011 - qperpy * s021 + qperpw * (-s001 + s101)
|
|
- 2. * qperpz * s111
|
|
+ qperpy * s121
|
|
- 2. * q0w * s001 * theta
|
|
- 2. * q0y * s021 * theta),
|
|
-(q0x * qperpy * s002) - q0w * qperpz * s002
|
|
+ 2. * q0x * qperpx * s012
|
|
+ q0x * qperpw * s022
|
|
+ q0w * qperpx * s022
|
|
+ q0x * qperpy * s102
|
|
+ q0w * qperpz * s102
|
|
- 2. * q0x * qperpx * s112
|
|
- q0x * qperpw * s122
|
|
- q0w * qperpx * s122
|
|
+ 2. * qperpx * qperpy * s002 * theta
|
|
+ 2. * qperpw * qperpz * s002 * theta
|
|
+ 2. * q0x * q0x * s012 * theta
|
|
+ 2. * q0z * q0z * s012 * theta
|
|
- 2. * qperpx * qperpx * s012 * theta
|
|
- 2. * qperpz * qperpz * s012 * theta
|
|
+ 2. * q0w * q0x * s022 * theta
|
|
- 2. * qperpw * qperpx * s022 * theta
|
|
+ 2. * qperpy * qperpz * s022 * theta
|
|
+ q0y
|
|
* (-(qperpx * s002) - qperpz * s022 + qperpx * s102 + qperpz * s122
|
|
- 2. * q0x * s002 * theta)
|
|
+ q0z
|
|
* (2. * qperpz * s012 - qperpy * s022 + qperpw * (-s002 + s102)
|
|
- 2. * qperpz * s112
|
|
+ qperpy * s122
|
|
- 2. * q0w * s002 * theta
|
|
- 2. * q0y * s022 * theta),
|
|
);
|
|
|
|
c5[1] = DerivativeTerm::new(
|
|
0.,
|
|
-2. * (qperpx * qperpy * s000 + qperpw * qperpz * s000 + q0z * q0z * s010
|
|
- qperpx * qperpx * s010
|
|
- qperpz * qperpz * s010
|
|
- q0y * q0z * s020
|
|
- qperpw * qperpx * s020
|
|
+ qperpy * qperpz * s020
|
|
- qperpx * qperpy * s100
|
|
- qperpw * qperpz * s100
|
|
+ q0w * q0z * (-s000 + s100)
|
|
+ q0x * q0x * (s010 - s110)
|
|
- q0z * q0z * s110
|
|
+ qperpx * qperpx * s110
|
|
+ qperpz * qperpz * s110
|
|
+ q0x * (q0y * (-s000 + s100) + q0w * (s020 - s120))
|
|
+ q0y * q0z * s120
|
|
+ qperpw * qperpx * s120
|
|
- qperpy * qperpz * s120)
|
|
* theta,
|
|
-2. * (qperpx * qperpy * s001 + qperpw * qperpz * s001 + q0z * q0z * s011
|
|
- qperpx * qperpx * s011
|
|
- qperpz * qperpz * s011
|
|
- q0y * q0z * s021
|
|
- qperpw * qperpx * s021
|
|
+ qperpy * qperpz * s021
|
|
- qperpx * qperpy * s101
|
|
- qperpw * qperpz * s101
|
|
+ q0w * q0z * (-s001 + s101)
|
|
+ q0x * q0x * (s011 - s111)
|
|
- q0z * q0z * s111
|
|
+ qperpx * qperpx * s111
|
|
+ qperpz * qperpz * s111
|
|
+ q0x * (q0y * (-s001 + s101) + q0w * (s021 - s121))
|
|
+ q0y * q0z * s121
|
|
+ qperpw * qperpx * s121
|
|
- qperpy * qperpz * s121)
|
|
* theta,
|
|
-2. * (qperpx * qperpy * s002 + qperpw * qperpz * s002 + q0z * q0z * s012
|
|
- qperpx * qperpx * s012
|
|
- qperpz * qperpz * s012
|
|
- q0y * q0z * s022
|
|
- qperpw * qperpx * s022
|
|
+ qperpy * qperpz * s022
|
|
- qperpx * qperpy * s102
|
|
- qperpw * qperpz * s102
|
|
+ q0w * q0z * (-s002 + s102)
|
|
+ q0x * q0x * (s012 - s112)
|
|
- q0z * q0z * s112
|
|
+ qperpx * qperpx * s112
|
|
+ qperpz * qperpz * s112
|
|
+ q0x * (q0y * (-s002 + s102) + q0w * (s022 - s122))
|
|
+ q0y * q0z * s122
|
|
+ qperpw * qperpx * s122
|
|
- qperpy * qperpz * s122)
|
|
* theta,
|
|
);
|
|
|
|
c1[2] = DerivativeTerm::new(
|
|
-t0z + t1z,
|
|
qperpw * qperpy * s000
|
|
- qperpx * qperpz * s000
|
|
- q0y * q0z * s010
|
|
- qperpw * qperpx * s010
|
|
- qperpy * qperpz * s010
|
|
- s020
|
|
+ q0y * q0y * s020
|
|
+ qperpx * qperpx * s020
|
|
+ qperpy * qperpy * s020
|
|
- qperpw * qperpy * s100
|
|
+ qperpx * qperpz * s100
|
|
+ q0x * q0z * (-s000 + s100)
|
|
+ q0y * q0z * s110
|
|
+ qperpw * qperpx * s110
|
|
+ qperpy * qperpz * s110
|
|
+ q0w * (q0y * (s000 - s100) + q0x * (-s010 + s110))
|
|
+ q0x * q0x * (s020 - s120)
|
|
+ s120
|
|
- q0y * q0y * s120
|
|
- qperpx * qperpx * s120
|
|
- qperpy * qperpy * s120,
|
|
qperpw * qperpy * s001
|
|
- qperpx * qperpz * s001
|
|
- q0y * q0z * s011
|
|
- qperpw * qperpx * s011
|
|
- qperpy * qperpz * s011
|
|
- s021
|
|
+ q0y * q0y * s021
|
|
+ qperpx * qperpx * s021
|
|
+ qperpy * qperpy * s021
|
|
- qperpw * qperpy * s101
|
|
+ qperpx * qperpz * s101
|
|
+ q0x * q0z * (-s001 + s101)
|
|
+ q0y * q0z * s111
|
|
+ qperpw * qperpx * s111
|
|
+ qperpy * qperpz * s111
|
|
+ q0w * (q0y * (s001 - s101) + q0x * (-s011 + s111))
|
|
+ q0x * q0x * (s021 - s121)
|
|
+ s121
|
|
- q0y * q0y * s121
|
|
- qperpx * qperpx * s121
|
|
- qperpy * qperpy * s121,
|
|
qperpw * qperpy * s002
|
|
- qperpx * qperpz * s002
|
|
- q0y * q0z * s012
|
|
- qperpw * qperpx * s012
|
|
- qperpy * qperpz * s012
|
|
- s022
|
|
+ q0y * q0y * s022
|
|
+ qperpx * qperpx * s022
|
|
+ qperpy * qperpy * s022
|
|
- qperpw * qperpy * s102
|
|
+ qperpx * qperpz * s102
|
|
+ q0x * q0z * (-s002 + s102)
|
|
+ q0y * q0z * s112
|
|
+ qperpw * qperpx * s112
|
|
+ qperpy * qperpz * s112
|
|
+ q0w * (q0y * (s002 - s102) + q0x * (-s012 + s112))
|
|
+ q0x * q0x * (s022 - s122)
|
|
+ s122
|
|
- q0y * q0y * s122
|
|
- qperpx * qperpx * s122
|
|
- qperpy * qperpy * s122,
|
|
);
|
|
|
|
c2[2] = DerivativeTerm::new(
|
|
0.,
|
|
q0w * q0y * s000 - q0x * q0z * s000 - qperpw * qperpy * s000
|
|
+ qperpx * qperpz * s000
|
|
- q0w * q0x * s010
|
|
- q0y * q0z * s010
|
|
+ qperpw * qperpx * s010
|
|
+ qperpy * qperpz * s010
|
|
+ q0x * q0x * s020
|
|
+ q0y * q0y * s020
|
|
- qperpx * qperpx * s020
|
|
- qperpy * qperpy * s020
|
|
- q0w * q0y * s100
|
|
+ q0x * q0z * s100
|
|
+ qperpw * qperpy * s100
|
|
- qperpx * qperpz * s100
|
|
+ q0w * q0x * s110
|
|
+ q0y * q0z * s110
|
|
- qperpw * qperpx * s110
|
|
- qperpy * qperpz * s110
|
|
- q0x * q0x * s120
|
|
- q0y * q0y * s120
|
|
+ qperpx * qperpx * s120
|
|
+ qperpy * qperpy * s120
|
|
- 2. * q0y * qperpw * s000 * theta
|
|
+ 2. * q0z * qperpx * s000 * theta
|
|
- 2. * q0w * qperpy * s000 * theta
|
|
+ 2. * q0x * qperpz * s000 * theta
|
|
+ 2. * q0x * qperpw * s010 * theta
|
|
+ 2. * q0w * qperpx * s010 * theta
|
|
+ 2. * q0z * qperpy * s010 * theta
|
|
+ 2. * q0y * qperpz * s010 * theta
|
|
- 4. * q0x * qperpx * s020 * theta
|
|
- 4. * q0y * qperpy * s020 * theta,
|
|
q0w * q0y * s001 - q0x * q0z * s001 - qperpw * qperpy * s001
|
|
+ qperpx * qperpz * s001
|
|
- q0w * q0x * s011
|
|
- q0y * q0z * s011
|
|
+ qperpw * qperpx * s011
|
|
+ qperpy * qperpz * s011
|
|
+ q0x * q0x * s021
|
|
+ q0y * q0y * s021
|
|
- qperpx * qperpx * s021
|
|
- qperpy * qperpy * s021
|
|
- q0w * q0y * s101
|
|
+ q0x * q0z * s101
|
|
+ qperpw * qperpy * s101
|
|
- qperpx * qperpz * s101
|
|
+ q0w * q0x * s111
|
|
+ q0y * q0z * s111
|
|
- qperpw * qperpx * s111
|
|
- qperpy * qperpz * s111
|
|
- q0x * q0x * s121
|
|
- q0y * q0y * s121
|
|
+ qperpx * qperpx * s121
|
|
+ qperpy * qperpy * s121
|
|
- 2. * q0y * qperpw * s001 * theta
|
|
+ 2. * q0z * qperpx * s001 * theta
|
|
- 2. * q0w * qperpy * s001 * theta
|
|
+ 2. * q0x * qperpz * s001 * theta
|
|
+ 2. * q0x * qperpw * s011 * theta
|
|
+ 2. * q0w * qperpx * s011 * theta
|
|
+ 2. * q0z * qperpy * s011 * theta
|
|
+ 2. * q0y * qperpz * s011 * theta
|
|
- 4. * q0x * qperpx * s021 * theta
|
|
- 4. * q0y * qperpy * s021 * theta,
|
|
q0w * q0y * s002 - q0x * q0z * s002 - qperpw * qperpy * s002
|
|
+ qperpx * qperpz * s002
|
|
- q0w * q0x * s012
|
|
- q0y * q0z * s012
|
|
+ qperpw * qperpx * s012
|
|
+ qperpy * qperpz * s012
|
|
+ q0x * q0x * s022
|
|
+ q0y * q0y * s022
|
|
- qperpx * qperpx * s022
|
|
- qperpy * qperpy * s022
|
|
- q0w * q0y * s102
|
|
+ q0x * q0z * s102
|
|
+ qperpw * qperpy * s102
|
|
- qperpx * qperpz * s102
|
|
+ q0w * q0x * s112
|
|
+ q0y * q0z * s112
|
|
- qperpw * qperpx * s112
|
|
- qperpy * qperpz * s112
|
|
- q0x * q0x * s122
|
|
- q0y * q0y * s122
|
|
+ qperpx * qperpx * s122
|
|
+ qperpy * qperpy * s122
|
|
- 2. * q0y * qperpw * s002 * theta
|
|
+ 2. * q0z * qperpx * s002 * theta
|
|
- 2. * q0w * qperpy * s002 * theta
|
|
+ 2. * q0x * qperpz * s002 * theta
|
|
+ 2. * q0x * qperpw * s012 * theta
|
|
+ 2. * q0w * qperpx * s012 * theta
|
|
+ 2. * q0z * qperpy * s012 * theta
|
|
+ 2. * q0y * qperpz * s012 * theta
|
|
- 4. * q0x * qperpx * s022 * theta
|
|
- 4. * q0y * qperpy * s022 * theta,
|
|
);
|
|
|
|
c3[2] = DerivativeTerm::new(
|
|
0.,
|
|
-2. * (-(q0w * qperpy * s000)
|
|
+ q0x * qperpz * s000
|
|
+ q0x * qperpw * s010
|
|
+ q0w * qperpx * s010
|
|
- 2. * q0x * qperpx * s020
|
|
+ q0w * qperpy * s100
|
|
- q0x * qperpz * s100
|
|
- q0x * qperpw * s110
|
|
- q0w * qperpx * s110
|
|
+ q0z * (qperpx * s000 + qperpy * s010 - qperpx * s100 - qperpy * s110)
|
|
+ 2. * q0x * qperpx * s120
|
|
+ q0y
|
|
* (qperpz * s010 - 2. * qperpy * s020 + qperpw * (-s000 + s100)
|
|
- qperpz * s110
|
|
+ 2. * qperpy * s120))
|
|
* theta,
|
|
-2. * (-(q0w * qperpy * s001)
|
|
+ q0x * qperpz * s001
|
|
+ q0x * qperpw * s011
|
|
+ q0w * qperpx * s011
|
|
- 2. * q0x * qperpx * s021
|
|
+ q0w * qperpy * s101
|
|
- q0x * qperpz * s101
|
|
- q0x * qperpw * s111
|
|
- q0w * qperpx * s111
|
|
+ q0z * (qperpx * s001 + qperpy * s011 - qperpx * s101 - qperpy * s111)
|
|
+ 2. * q0x * qperpx * s121
|
|
+ q0y
|
|
* (qperpz * s011 - 2. * qperpy * s021 + qperpw * (-s001 + s101)
|
|
- qperpz * s111
|
|
+ 2. * qperpy * s121))
|
|
* theta,
|
|
-2. * (-(q0w * qperpy * s002)
|
|
+ q0x * qperpz * s002
|
|
+ q0x * qperpw * s012
|
|
+ q0w * qperpx * s012
|
|
- 2. * q0x * qperpx * s022
|
|
+ q0w * qperpy * s102
|
|
- q0x * qperpz * s102
|
|
- q0x * qperpw * s112
|
|
- q0w * qperpx * s112
|
|
+ q0z * (qperpx * s002 + qperpy * s012 - qperpx * s102 - qperpy * s112)
|
|
+ 2. * q0x * qperpx * s122
|
|
+ q0y
|
|
* (qperpz * s012 - 2. * qperpy * s022 + qperpw * (-s002 + s102)
|
|
- qperpz * s112
|
|
+ 2. * qperpy * s122))
|
|
* theta,
|
|
);
|
|
|
|
c4[2] = DerivativeTerm::new(
|
|
0.,
|
|
q0w * qperpy * s000
|
|
- q0x * qperpz * s000
|
|
- q0x * qperpw * s010
|
|
- q0w * qperpx * s010
|
|
+ 2. * q0x * qperpx * s020
|
|
- q0w * qperpy * s100
|
|
+ q0x * qperpz * s100
|
|
+ q0x * qperpw * s110
|
|
+ q0w * qperpx * s110
|
|
- 2. * q0x * qperpx * s120
|
|
- 2. * qperpw * qperpy * s000 * theta
|
|
+ 2. * qperpx * qperpz * s000 * theta
|
|
- 2. * q0w * q0x * s010 * theta
|
|
+ 2. * qperpw * qperpx * s010 * theta
|
|
+ 2. * qperpy * qperpz * s010 * theta
|
|
+ 2. * q0x * q0x * s020 * theta
|
|
+ 2. * q0y * q0y * s020 * theta
|
|
- 2. * qperpx * qperpx * s020 * theta
|
|
- 2. * qperpy * qperpy * s020 * theta
|
|
+ q0z
|
|
* (-(qperpx * s000) - qperpy * s010 + qperpx * s100 + qperpy * s110
|
|
- 2. * q0x * s000 * theta)
|
|
+ q0y
|
|
* (-(qperpz * s010)
|
|
+ 2. * qperpy * s020
|
|
+ qperpw * (s000 - s100)
|
|
+ qperpz * s110
|
|
- 2. * qperpy * s120
|
|
+ 2. * q0w * s000 * theta
|
|
- 2. * q0z * s010 * theta),
|
|
q0w * qperpy * s001
|
|
- q0x * qperpz * s001
|
|
- q0x * qperpw * s011
|
|
- q0w * qperpx * s011
|
|
+ 2. * q0x * qperpx * s021
|
|
- q0w * qperpy * s101
|
|
+ q0x * qperpz * s101
|
|
+ q0x * qperpw * s111
|
|
+ q0w * qperpx * s111
|
|
- 2. * q0x * qperpx * s121
|
|
- 2. * qperpw * qperpy * s001 * theta
|
|
+ 2. * qperpx * qperpz * s001 * theta
|
|
- 2. * q0w * q0x * s011 * theta
|
|
+ 2. * qperpw * qperpx * s011 * theta
|
|
+ 2. * qperpy * qperpz * s011 * theta
|
|
+ 2. * q0x * q0x * s021 * theta
|
|
+ 2. * q0y * q0y * s021 * theta
|
|
- 2. * qperpx * qperpx * s021 * theta
|
|
- 2. * qperpy * qperpy * s021 * theta
|
|
+ q0z
|
|
* (-(qperpx * s001) - qperpy * s011 + qperpx * s101 + qperpy * s111
|
|
- 2. * q0x * s001 * theta)
|
|
+ q0y
|
|
* (-(qperpz * s011)
|
|
+ 2. * qperpy * s021
|
|
+ qperpw * (s001 - s101)
|
|
+ qperpz * s111
|
|
- 2. * qperpy * s121
|
|
+ 2. * q0w * s001 * theta
|
|
- 2. * q0z * s011 * theta),
|
|
q0w * qperpy * s002
|
|
- q0x * qperpz * s002
|
|
- q0x * qperpw * s012
|
|
- q0w * qperpx * s012
|
|
+ 2. * q0x * qperpx * s022
|
|
- q0w * qperpy * s102
|
|
+ q0x * qperpz * s102
|
|
+ q0x * qperpw * s112
|
|
+ q0w * qperpx * s112
|
|
- 2. * q0x * qperpx * s122
|
|
- 2. * qperpw * qperpy * s002 * theta
|
|
+ 2. * qperpx * qperpz * s002 * theta
|
|
- 2. * q0w * q0x * s012 * theta
|
|
+ 2. * qperpw * qperpx * s012 * theta
|
|
+ 2. * qperpy * qperpz * s012 * theta
|
|
+ 2. * q0x * q0x * s022 * theta
|
|
+ 2. * q0y * q0y * s022 * theta
|
|
- 2. * qperpx * qperpx * s022 * theta
|
|
- 2. * qperpy * qperpy * s022 * theta
|
|
+ q0z
|
|
* (-(qperpx * s002) - qperpy * s012 + qperpx * s102 + qperpy * s112
|
|
- 2. * q0x * s002 * theta)
|
|
+ q0y
|
|
* (-(qperpz * s012)
|
|
+ 2. * qperpy * s022
|
|
+ qperpw * (s002 - s102)
|
|
+ qperpz * s112
|
|
- 2. * qperpy * s122
|
|
+ 2. * q0w * s002 * theta
|
|
- 2. * q0z * s012 * theta),
|
|
);
|
|
|
|
c5[2] = DerivativeTerm::new(
|
|
0.,
|
|
2. * (qperpw * qperpy * s000 - qperpx * qperpz * s000 + q0y * q0z * s010
|
|
- qperpw * qperpx * s010
|
|
- qperpy * qperpz * s010
|
|
- q0y * q0y * s020
|
|
+ qperpx * qperpx * s020
|
|
+ qperpy * qperpy * s020
|
|
+ q0x * q0z * (s000 - s100)
|
|
- qperpw * qperpy * s100
|
|
+ qperpx * qperpz * s100
|
|
+ q0w * (q0y * (-s000 + s100) + q0x * (s010 - s110))
|
|
- q0y * q0z * s110
|
|
+ qperpw * qperpx * s110
|
|
+ qperpy * qperpz * s110
|
|
+ q0y * q0y * s120
|
|
- qperpx * qperpx * s120
|
|
- qperpy * qperpy * s120
|
|
+ q0x * q0x * (-s020 + s120))
|
|
* theta,
|
|
2. * (qperpw * qperpy * s001 - qperpx * qperpz * s001 + q0y * q0z * s011
|
|
- qperpw * qperpx * s011
|
|
- qperpy * qperpz * s011
|
|
- q0y * q0y * s021
|
|
+ qperpx * qperpx * s021
|
|
+ qperpy * qperpy * s021
|
|
+ q0x * q0z * (s001 - s101)
|
|
- qperpw * qperpy * s101
|
|
+ qperpx * qperpz * s101
|
|
+ q0w * (q0y * (-s001 + s101) + q0x * (s011 - s111))
|
|
- q0y * q0z * s111
|
|
+ qperpw * qperpx * s111
|
|
+ qperpy * qperpz * s111
|
|
+ q0y * q0y * s121
|
|
- qperpx * qperpx * s121
|
|
- qperpy * qperpy * s121
|
|
+ q0x * q0x * (-s021 + s121))
|
|
* theta,
|
|
2. * (qperpw * qperpy * s002 - qperpx * qperpz * s002 + q0y * q0z * s012
|
|
- qperpw * qperpx * s012
|
|
- qperpy * qperpz * s012
|
|
- q0y * q0y * s022
|
|
+ qperpx * qperpx * s022
|
|
+ qperpy * qperpy * s022
|
|
+ q0x * q0z * (s002 - s102)
|
|
- qperpw * qperpy * s102
|
|
+ qperpx * qperpz * s102
|
|
+ q0w * (q0y * (-s002 + s102) + q0x * (s012 - s112))
|
|
- q0y * q0z * s112
|
|
+ qperpw * qperpx * s112
|
|
+ qperpy * qperpz * s112
|
|
+ q0y * q0y * s122
|
|
- qperpx * qperpx * s122
|
|
- qperpy * qperpy * s122
|
|
+ q0x * q0x * (-s022 + s122))
|
|
* theta,
|
|
);
|
|
(c1, c2, c3, c4, c5)
|
|
} else {
|
|
(
|
|
gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
gpu_array_from_fn(|_| DerivativeTerm::default()),
|
|
)
|
|
};
|
|
AnimatedTransform {
|
|
start_transform: *start_transform,
|
|
end_transform: *end_transform,
|
|
start_time,
|
|
end_time,
|
|
actually_animated,
|
|
t,
|
|
r,
|
|
s,
|
|
has_rotation,
|
|
c1,
|
|
c2,
|
|
c3,
|
|
c4,
|
|
c5,
|
|
}
|
|
}
|
|
|
|
pub fn apply_point(&self, p: Point3f, time: Float) -> Point3f {
|
|
if !self.actually_animated || time <= self.start_time {
|
|
return self.start_transform.apply_to_point(p);
|
|
} else if time >= self.end_time {
|
|
return self.end_transform.apply_to_point(p);
|
|
}
|
|
let t = self.interpolate(time);
|
|
t.apply_to_point(p)
|
|
}
|
|
|
|
pub fn apply_vector(&self, v: Vector3f, time: Float) -> Vector3f {
|
|
if !self.actually_animated || time <= self.start_time {
|
|
return self.start_transform.apply_to_vector(v);
|
|
} else if time >= self.end_time {
|
|
return self.end_transform.apply_to_vector(v);
|
|
}
|
|
let t = self.interpolate(time);
|
|
t.apply_to_vector(v)
|
|
}
|
|
|
|
pub fn apply_ray(&self, r: &Ray, t_max: &mut Option<Float>) -> Ray {
|
|
if !self.actually_animated || r.time <= self.start_time {
|
|
return self.start_transform.apply_to_ray(r, t_max);
|
|
} else if r.time >= self.end_time {
|
|
return self.end_transform.apply_to_ray(r, t_max);
|
|
}
|
|
let t = self.interpolate(r.time);
|
|
t.apply_to_ray(r, t_max)
|
|
}
|
|
|
|
pub fn apply_interaction(&self, si: &Interaction) -> Interaction {
|
|
if !self.actually_animated {
|
|
return self.start_transform.apply_to_interaction(si);
|
|
}
|
|
let t = self.interpolate(si.time());
|
|
t.apply_to_interaction(si)
|
|
}
|
|
|
|
pub fn interpolate(&self, time: Float) -> TransformGeneric<Float> {
|
|
if !self.actually_animated || time <= self.start_time {
|
|
return self.start_transform;
|
|
}
|
|
if time >= self.end_time {
|
|
return self.end_transform;
|
|
}
|
|
|
|
let dt = (time - self.start_time) / (self.end_time - self.start_time);
|
|
let trans = (1.0 - dt) * self.t[0] + dt * self.t[1];
|
|
|
|
let rotate = Quaternion::slerp(dt, self.r[0], self.r[1]);
|
|
|
|
let scale = (1.0 - dt) * self.s[0] + dt * self.s[1];
|
|
let scale_transform =
|
|
TransformGeneric::from_matrix(scale).expect("Scale matrix is not inversible");
|
|
|
|
TransformGeneric::translate(trans) * TransformGeneric::from(rotate) * scale_transform
|
|
}
|
|
|
|
pub fn apply_inverse_point(&self, p: Point3f, time: Float) -> Point3f {
|
|
if !self.actually_animated {
|
|
return self.start_transform.apply_inverse(p);
|
|
}
|
|
self.interpolate(time).apply_inverse(p)
|
|
}
|
|
|
|
pub fn apply_inverse_vector(&self, v: Vector3f, time: Float) -> Vector3f {
|
|
if !self.actually_animated {
|
|
return self.start_transform.apply_inverse_vector(v);
|
|
}
|
|
self.interpolate(time).apply_inverse_vector(v)
|
|
}
|
|
|
|
pub fn apply_inverse_normal(&self, n: Normal3f, time: Float) -> Normal3f {
|
|
if !self.actually_animated {
|
|
return self.start_transform.apply_inverse_normal(n);
|
|
}
|
|
self.interpolate(time).apply_inverse_normal(n)
|
|
}
|
|
|
|
pub fn motion_bounds(&self, b: &Bounds3f) -> Bounds3f {
|
|
if !self.actually_animated {
|
|
return self.start_transform.apply_to_bounds(*b);
|
|
}
|
|
self.start_transform
|
|
.apply_to_bounds(*b)
|
|
.union(self.end_transform.apply_to_bounds(*b))
|
|
}
|
|
|
|
pub fn is_animated(&self) -> bool {
|
|
self.actually_animated
|
|
}
|
|
}
|
|
|
|
pub fn look_at(
|
|
pos: impl Into<Point3f>,
|
|
look: impl Into<Point3f>,
|
|
up: impl Into<Point3f>,
|
|
) -> Option<TransformGeneric<Float>> {
|
|
let mut world_from_camera: SquareMatrix<Float, 4> = SquareMatrix::default();
|
|
// Initialize fourth column of viewing matrix
|
|
let pos: Point3f = pos.into();
|
|
let look: Point3f = look.into();
|
|
let up: Point3f = up.into();
|
|
world_from_camera[0][3] = pos.x();
|
|
world_from_camera[1][3] = pos.y();
|
|
world_from_camera[2][3] = pos.z();
|
|
world_from_camera[3][3] = 1.;
|
|
|
|
// Initialize first three columns of viewing matrix
|
|
let dir = (look - pos).normalize();
|
|
if Vector3f::from(up).normalize().cross(dir).norm() == 0. {
|
|
panic!(
|
|
"LookAt: \"up\" vector ({}, {}, {}) and viewing direction ({}, {}, {}) passed to LookAt are pointing in the same direction.",
|
|
up.x(),
|
|
up.y(),
|
|
up.z(),
|
|
dir.x(),
|
|
dir.y(),
|
|
dir.z()
|
|
);
|
|
}
|
|
let right = Vector3f::from(up).normalize().cross(dir).normalize();
|
|
let new_up = dir.cross(right);
|
|
world_from_camera[0][0] = right.x();
|
|
world_from_camera[1][0] = right.y();
|
|
world_from_camera[2][0] = right.z();
|
|
world_from_camera[3][0] = 0.;
|
|
world_from_camera[0][1] = new_up.x();
|
|
world_from_camera[1][1] = new_up.y();
|
|
world_from_camera[2][1] = new_up.z();
|
|
world_from_camera[3][1] = 0.;
|
|
world_from_camera[0][2] = dir.x();
|
|
world_from_camera[1][2] = dir.y();
|
|
world_from_camera[2][2] = dir.z();
|
|
world_from_camera[3][2] = 0.;
|
|
|
|
let camera_from_world = world_from_camera.inverse()?;
|
|
Some(TransformGeneric::new(camera_from_world, world_from_camera))
|
|
}
|