use glam::{DQuat, DVec3, Quat, Vec3};
#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Pose {
pub position: DVec3,
pub orientation: DQuat,
}
impl Default for Pose {
fn default() -> Self {
Self::IDENTITY
}
}
impl Pose {
pub const IDENTITY: Pose = Pose {
position: DVec3::ZERO,
orientation: DQuat::IDENTITY,
};
pub fn new(position: impl Into<DVec3>, orientation: DQuat) -> Self {
Self {
position: position.into(),
orientation,
}
}
pub fn orbit(centre: impl Into<DVec3>, distance: f32, orientation: Quat) -> Self {
Self {
position: centre.into() + eye_offset(orientation, distance),
orientation: orientation.as_dquat(),
}
}
pub fn look_at(eye: impl Into<DVec3>, target: impl Into<DVec3>, up: Vec3) -> Self {
let eye = eye.into();
let forward = (target.into() - eye).normalize_or(DVec3::NEG_Z);
let mut up = up.as_dvec3().normalize_or(DVec3::Z);
if forward.cross(up).length_squared() < 1e-12 {
up = if forward.z.abs() < 0.9 {
DVec3::Z
} else {
DVec3::Y
};
}
let right = forward.cross(up).normalize();
let up = right.cross(forward);
let basis = glam::DMat3::from_cols(right, up, -forward);
Self {
position: eye,
orientation: DQuat::from_mat3(&basis).normalize(),
}
}
pub fn orientation_f32(&self) -> Quat {
self.orientation.as_quat()
}
pub fn forward(&self) -> Vec3 {
-(self.orientation_f32() * Vec3::Z)
}
pub fn right(&self) -> Vec3 {
self.orientation_f32() * Vec3::X
}
pub fn up(&self) -> Vec3 {
self.orientation_f32() * Vec3::Y
}
pub fn target_at(&self, distance: f32) -> DVec3 {
self.position - eye_offset(self.orientation_f32(), distance)
}
}
pub(crate) fn eye_offset(orientation: Quat, distance: f32) -> DVec3 {
(orientation * (Vec3::Z * distance)).as_dvec3()
}