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))
}