665 lines
18 KiB
Rust
665 lines
18 KiB
Rust
pub use fixed_wide::fixed::{Fixed,Fix};
|
|
pub use ratio_ops::ratio::{Ratio,Divide};
|
|
|
|
//integer units
|
|
#[derive(Clone,Copy,Hash,Eq,PartialEq,PartialOrd,Debug)]
|
|
pub struct Time(i64);
|
|
impl Time{
|
|
pub const MIN:Self=Self(i64::MIN);
|
|
pub const MAX:Self=Self(i64::MAX);
|
|
pub const ZERO:Self=Self(0);
|
|
pub const ONE_SECOND:Self=Self(1_000_000_000);
|
|
pub const ONE_MILLISECOND:Self=Self(1_000_000);
|
|
pub const ONE_MICROSECOND:Self=Self(1_000);
|
|
pub const ONE_NANOSECOND:Self=Self(1);
|
|
#[inline]
|
|
pub const fn raw(num:i64)->Self{
|
|
Self(num)
|
|
}
|
|
#[inline]
|
|
pub const fn get(self)->i64{
|
|
self.0
|
|
}
|
|
#[inline]
|
|
pub const fn from_secs(num:i64)->Self{
|
|
Self(Self::ONE_SECOND.0*num)
|
|
}
|
|
#[inline]
|
|
pub const fn from_millis(num:i64)->Self{
|
|
Self(Self::ONE_MILLISECOND.0*num)
|
|
}
|
|
#[inline]
|
|
pub const fn from_micros(num:i64)->Self{
|
|
Self(Self::ONE_MICROSECOND.0*num)
|
|
}
|
|
#[inline]
|
|
pub const fn from_nanos(num:i64)->Self{
|
|
Self(Self::ONE_NANOSECOND.0*num)
|
|
}
|
|
//should I have checked subtraction? force all time variables to be positive?
|
|
#[inline]
|
|
pub const fn nanos(self)->i64{
|
|
self.0
|
|
}
|
|
#[inline]
|
|
pub const fn to_ratio(self)->Ratio<Planar64,Planar64>{
|
|
Ratio::new(Planar64::raw(self.0),Planar64::raw(1_000_000_000))
|
|
}
|
|
}
|
|
impl From<Planar64> for Time{
|
|
#[inline]
|
|
fn from(value:Planar64)->Self{
|
|
Time((value*Planar64::raw(1_000_000_000)).fix_1().to_raw())
|
|
}
|
|
}
|
|
impl<Num,Den,N1,T1> From<Ratio<Num,Den>> for Time
|
|
where
|
|
Num:core::ops::Mul<Planar64,Output=N1>,
|
|
N1:Divide<Den,Output=T1>,
|
|
T1:Fix<Planar64>,
|
|
{
|
|
#[inline]
|
|
fn from(value:Ratio<Num,Den>)->Self{
|
|
Time((value*Planar64::raw(1_000_000_000)).divide().fix().to_raw())
|
|
}
|
|
}
|
|
impl std::fmt::Display for Time{
|
|
#[inline]
|
|
fn fmt(&self,f:&mut std::fmt::Formatter<'_>)->std::fmt::Result{
|
|
write!(f,"{}s+{:09}ns",self.0/Self::ONE_SECOND.0,self.0%Self::ONE_SECOND.0)
|
|
}
|
|
}
|
|
impl std::default::Default for Time{
|
|
fn default()->Self{
|
|
Self(0)
|
|
}
|
|
}
|
|
impl std::ops::Neg for Time{
|
|
type Output=Time;
|
|
#[inline]
|
|
fn neg(self)->Self::Output {
|
|
Time(-self.0)
|
|
}
|
|
}
|
|
macro_rules! impl_time_additive_operator {
|
|
($trait:ty, $method:ident) => {
|
|
impl $trait for Time{
|
|
type Output=Time;
|
|
#[inline]
|
|
fn $method(self,rhs:Self)->Self::Output {
|
|
Time(self.0.$method(rhs.0))
|
|
}
|
|
}
|
|
};
|
|
}
|
|
impl_time_additive_operator!(core::ops::Add,add);
|
|
impl_time_additive_operator!(core::ops::Sub,sub);
|
|
impl_time_additive_operator!(core::ops::Rem,rem);
|
|
macro_rules! impl_time_additive_assign_operator {
|
|
($trait:ty, $method:ident) => {
|
|
impl $trait for Time{
|
|
#[inline]
|
|
fn $method(&mut self,rhs:Self){
|
|
self.0.$method(rhs.0)
|
|
}
|
|
}
|
|
};
|
|
}
|
|
impl_time_additive_assign_operator!(core::ops::AddAssign,add_assign);
|
|
impl_time_additive_assign_operator!(core::ops::SubAssign,sub_assign);
|
|
impl_time_additive_assign_operator!(core::ops::RemAssign,rem_assign);
|
|
impl std::ops::Mul for Time{
|
|
type Output=Ratio<fixed_wide::fixed::Fixed<2,64>,fixed_wide::fixed::Fixed<2,64>>;
|
|
#[inline]
|
|
fn mul(self,rhs:Self)->Self::Output{
|
|
Ratio::new(Fixed::raw(self.0)*Fixed::raw(rhs.0),Fixed::raw_digit(1_000_000_000i64.pow(2)))
|
|
}
|
|
}
|
|
impl std::ops::Div<i64> for Time{
|
|
type Output=Time;
|
|
#[inline]
|
|
fn div(self,rhs:i64)->Self::Output{
|
|
Time(self.0/rhs)
|
|
}
|
|
}
|
|
impl std::ops::Mul<i64> for Time{
|
|
type Output=Time;
|
|
#[inline]
|
|
fn mul(self,rhs:i64)->Self::Output{
|
|
Time(self.0*rhs)
|
|
}
|
|
}
|
|
impl core::ops::Mul<Time> for Planar64{
|
|
type Output=Ratio<Fixed<2,64>,Planar64>;
|
|
fn mul(self,rhs:Time)->Self::Output{
|
|
Ratio::new(self*Fixed::raw(rhs.0),Planar64::raw(1_000_000_000))
|
|
}
|
|
}
|
|
#[test]
|
|
fn time_from_planar64(){
|
|
let a:Time=Planar64::from(1).into();
|
|
assert_eq!(a,Time::ONE_SECOND);
|
|
}
|
|
#[test]
|
|
fn time_from_ratio(){
|
|
let a:Time=Ratio::new(Planar64::from(1),Planar64::from(1)).into();
|
|
assert_eq!(a,Time::ONE_SECOND);
|
|
}
|
|
#[test]
|
|
fn time_squared(){
|
|
let a=Time::from_secs(2);
|
|
assert_eq!(a*a,Ratio::new(Fixed::<2,64>::raw_digit(1_000_000_000i64.pow(2))*4,Fixed::<2,64>::raw_digit(1_000_000_000i64.pow(2))));
|
|
}
|
|
#[test]
|
|
fn time_times_planar64(){
|
|
let a=Time::from_secs(2);
|
|
let b=Planar64::from(2);
|
|
assert_eq!(b*a,Ratio::new(Fixed::<2,64>::raw_digit(1_000_000_000*(1<<32))<<2,Fixed::<1,32>::raw_digit(1_000_000_000)));
|
|
}
|
|
|
|
#[inline]
|
|
const fn gcd(mut a:u64,mut b:u64)->u64{
|
|
while b!=0{
|
|
(a,b)=(b,a.rem_euclid(b));
|
|
};
|
|
a
|
|
}
|
|
#[derive(Clone,Copy,Debug,Hash)]
|
|
pub struct Ratio64{
|
|
num:i64,
|
|
den:u64,
|
|
}
|
|
impl Ratio64{
|
|
pub const ZERO:Self=Ratio64{num:0,den:1};
|
|
pub const ONE:Self=Ratio64{num:1,den:1};
|
|
#[inline]
|
|
pub const fn new(num:i64,den:u64)->Option<Ratio64>{
|
|
if den==0{
|
|
None
|
|
}else{
|
|
let d=gcd(num.unsigned_abs(),den);
|
|
Some(Self{num:num/(d as i64),den:den/d})
|
|
}
|
|
}
|
|
#[inline]
|
|
pub const fn num(self)->i64{
|
|
self.num
|
|
}
|
|
#[inline]
|
|
pub const fn den(self)->u64{
|
|
self.den
|
|
}
|
|
#[inline]
|
|
pub const fn mul_int(&self,rhs:i64)->i64{
|
|
rhs*self.num/(self.den as i64)
|
|
}
|
|
#[inline]
|
|
pub const fn rhs_div_int(&self,rhs:i64)->i64{
|
|
rhs*(self.den as i64)/self.num
|
|
}
|
|
#[inline]
|
|
pub const fn mul_ref(&self,rhs:&Ratio64)->Ratio64{
|
|
let (num,den)=(self.num*rhs.num,self.den*rhs.den);
|
|
let d=gcd(num.unsigned_abs(),den);
|
|
Self{
|
|
num:num/(d as i64),
|
|
den:den/d,
|
|
}
|
|
}
|
|
}
|
|
//from num_traits crate
|
|
#[inline]
|
|
fn integer_decode_f32(f: f32) -> (u64, i16, i8) {
|
|
let bits: u32 = f.to_bits();
|
|
let sign: i8 = if bits >> 31 == 0 { 1 } else { -1 };
|
|
let mut exponent: i16 = ((bits >> 23) & 0xff) as i16;
|
|
let mantissa = if exponent == 0 {
|
|
(bits & 0x7fffff) << 1
|
|
} else {
|
|
(bits & 0x7fffff) | 0x800000
|
|
};
|
|
// Exponent bias + mantissa shift
|
|
exponent -= 127 + 23;
|
|
(mantissa as u64, exponent, sign)
|
|
}
|
|
#[inline]
|
|
fn integer_decode_f64(f: f64) -> (u64, i16, i8) {
|
|
let bits: u64 = f.to_bits();
|
|
let sign: i8 = if bits >> 63 == 0 { 1 } else { -1 };
|
|
let mut exponent: i16 = ((bits >> 52) & 0x7ff) as i16;
|
|
let mantissa = if exponent == 0 {
|
|
(bits & 0xfffffffffffff) << 1
|
|
} else {
|
|
(bits & 0xfffffffffffff) | 0x10000000000000
|
|
};
|
|
// Exponent bias + mantissa shift
|
|
exponent -= 1023 + 52;
|
|
(mantissa, exponent, sign)
|
|
}
|
|
#[derive(Debug)]
|
|
pub enum Ratio64TryFromFloatError{
|
|
Nan,
|
|
Infinite,
|
|
Subnormal,
|
|
HighlyNegativeExponent(i16),
|
|
HighlyPositiveExponent(i16),
|
|
}
|
|
const MAX_DENOMINATOR:u128=u64::MAX as u128;
|
|
#[inline]
|
|
fn ratio64_from_mes((m,e,s):(u64,i16,i8))->Result<Ratio64,Ratio64TryFromFloatError>{
|
|
if e< -127{
|
|
//this can also just be zero
|
|
Err(Ratio64TryFromFloatError::HighlyNegativeExponent(e))
|
|
}else if e< -63{
|
|
//approximate input ratio within denominator limit
|
|
let mut target_num=m as u128;
|
|
let mut target_den=1u128<<-e;
|
|
|
|
let mut num=1;
|
|
let mut den=0;
|
|
let mut prev_num=0;
|
|
let mut prev_den=1;
|
|
|
|
while target_den!=0{
|
|
let whole=target_num/target_den;
|
|
(target_num,target_den)=(target_den,target_num-whole*target_den);
|
|
let new_num=whole*num+prev_num;
|
|
let new_den=whole*den+prev_den;
|
|
if MAX_DENOMINATOR<new_den{
|
|
break;
|
|
}else{
|
|
(prev_num,prev_den)=(num,den);
|
|
(num,den)=(new_num,new_den);
|
|
}
|
|
}
|
|
|
|
Ok(Ratio64::new(num as i64,den as u64).unwrap())
|
|
}else if e<0{
|
|
Ok(Ratio64::new((m as i64)*(s as i64),1<<-e).unwrap())
|
|
}else if (64-m.leading_zeros() as i16)+e<64{
|
|
Ok(Ratio64::new((m as i64)*(s as i64)*(1<<e),1).unwrap())
|
|
}else{
|
|
Err(Ratio64TryFromFloatError::HighlyPositiveExponent(e))
|
|
}
|
|
}
|
|
impl TryFrom<f32> for Ratio64{
|
|
type Error=Ratio64TryFromFloatError;
|
|
#[inline]
|
|
fn try_from(value:f32)->Result<Self,Self::Error>{
|
|
match value.classify(){
|
|
std::num::FpCategory::Nan=>Err(Self::Error::Nan),
|
|
std::num::FpCategory::Infinite=>Err(Self::Error::Infinite),
|
|
std::num::FpCategory::Zero=>Ok(Self::ZERO),
|
|
std::num::FpCategory::Subnormal
|
|
|std::num::FpCategory::Normal=>ratio64_from_mes(integer_decode_f32(value)),
|
|
}
|
|
}
|
|
}
|
|
impl TryFrom<f64> for Ratio64{
|
|
type Error=Ratio64TryFromFloatError;
|
|
#[inline]
|
|
fn try_from(value:f64)->Result<Self,Self::Error>{
|
|
match value.classify(){
|
|
std::num::FpCategory::Nan=>Err(Self::Error::Nan),
|
|
std::num::FpCategory::Infinite=>Err(Self::Error::Infinite),
|
|
std::num::FpCategory::Zero=>Ok(Self::ZERO),
|
|
std::num::FpCategory::Subnormal
|
|
|std::num::FpCategory::Normal=>ratio64_from_mes(integer_decode_f64(value)),
|
|
}
|
|
}
|
|
}
|
|
impl std::ops::Mul<Ratio64> for Ratio64{
|
|
type Output=Ratio64;
|
|
#[inline]
|
|
fn mul(self,rhs:Ratio64)->Self::Output{
|
|
let (num,den)=(self.num*rhs.num,self.den*rhs.den);
|
|
let d=gcd(num.unsigned_abs(),den);
|
|
Self{
|
|
num:num/(d as i64),
|
|
den:den/d,
|
|
}
|
|
}
|
|
}
|
|
impl std::ops::Mul<i64> for Ratio64{
|
|
type Output=Ratio64;
|
|
#[inline]
|
|
fn mul(self,rhs:i64)->Self::Output {
|
|
Self{
|
|
num:self.num*rhs,
|
|
den:self.den,
|
|
}
|
|
}
|
|
}
|
|
impl std::ops::Div<u64> for Ratio64{
|
|
type Output=Ratio64;
|
|
#[inline]
|
|
fn div(self,rhs:u64)->Self::Output {
|
|
Self{
|
|
num:self.num,
|
|
den:self.den*rhs,
|
|
}
|
|
}
|
|
}
|
|
#[derive(Clone,Copy,Debug,Hash)]
|
|
pub struct Ratio64Vec2{
|
|
pub x:Ratio64,
|
|
pub y:Ratio64,
|
|
}
|
|
impl Ratio64Vec2{
|
|
pub const ONE:Self=Self{x:Ratio64::ONE,y:Ratio64::ONE};
|
|
#[inline]
|
|
pub const fn new(x:Ratio64,y:Ratio64)->Self{
|
|
Self{x,y}
|
|
}
|
|
#[inline]
|
|
pub const fn mul_int(&self,rhs:glam::I64Vec2)->glam::I64Vec2{
|
|
glam::i64vec2(
|
|
self.x.mul_int(rhs.x),
|
|
self.y.mul_int(rhs.y),
|
|
)
|
|
}
|
|
}
|
|
impl std::ops::Mul<i64> for Ratio64Vec2{
|
|
type Output=Ratio64Vec2;
|
|
#[inline]
|
|
fn mul(self,rhs:i64)->Self::Output {
|
|
Self{
|
|
x:self.x*rhs,
|
|
y:self.y*rhs,
|
|
}
|
|
}
|
|
}
|
|
|
|
///[-pi,pi) = [-2^31,2^31-1]
|
|
#[derive(Clone,Copy,Hash)]
|
|
pub struct Angle32(i32);
|
|
impl Angle32{
|
|
const ANGLE32_TO_FLOAT64_RADIANS:f64=std::f64::consts::PI/((1i64<<31) as f64);
|
|
pub const FRAC_PI_2:Self=Self(1<<30);
|
|
pub const NEG_FRAC_PI_2:Self=Self(-1<<30);
|
|
pub const PI:Self=Self(-1<<31);
|
|
#[inline]
|
|
pub const fn wrap_from_i64(theta:i64)->Self{
|
|
//take lower bits
|
|
//note: this was checked on compiler explorer and compiles to 1 instruction!
|
|
Self(i32::from_ne_bytes(((theta&((1<<32)-1)) as u32).to_ne_bytes()))
|
|
}
|
|
#[inline]
|
|
pub fn clamp_from_i64(theta:i64)->Self{
|
|
//the assembly is a bit confusing for this, I thought it was checking the same thing twice
|
|
//but it's just checking and then overwriting the value for both upper and lower bounds.
|
|
Self(theta.clamp(i32::MIN as i64,i32::MAX as i64) as i32)
|
|
}
|
|
#[inline]
|
|
pub const fn get(&self)->i32{
|
|
self.0
|
|
}
|
|
/// Clamps the value towards the midpoint of the range.
|
|
/// Note that theta_min can be larger than theta_max and it will wrap clamp the other way around
|
|
#[inline]
|
|
pub fn clamp(&self,theta_min:Self,theta_max:Self)->Self{
|
|
//((max-min as u32)/2 as i32)+min
|
|
let midpoint=((
|
|
(theta_max.0 as u32)
|
|
.wrapping_sub(theta_min.0 as u32)
|
|
/2
|
|
) as i32)//(u32::MAX/2) as i32 ALWAYS works
|
|
.wrapping_add(theta_min.0);
|
|
//(theta-mid).clamp(max-mid,min-mid)+mid
|
|
Self(
|
|
self.0.wrapping_sub(midpoint)
|
|
.max(theta_min.0.wrapping_sub(midpoint))
|
|
.min(theta_max.0.wrapping_sub(midpoint))
|
|
.wrapping_add(midpoint)
|
|
)
|
|
}
|
|
#[inline]
|
|
pub fn cos_sin(&self)->(Planar64,Planar64){
|
|
/*
|
|
//cordic
|
|
let a=self.0 as u32;
|
|
//initialize based on the quadrant
|
|
let (mut x,mut y)=match (a&(1<<31)!=0,a&(1<<30)!=0){
|
|
(false,false)=>( 1i64<<32, 0i64 ),//TR
|
|
(false,true )=>( 0i64 , 1i64<<32),//TL
|
|
(true ,false)=>(-1i64<<32, 0i64 ),//BL
|
|
(true ,true )=>( 0i64 ,-1i64<<32),//BR
|
|
};
|
|
println!("x={} y={}",Planar64::raw(x),Planar64::raw(y));
|
|
for i in 0..30{
|
|
if a&(1<<(29-i))!=0{
|
|
(x,y)=(x-(y>>i),y+(x>>i));
|
|
}
|
|
println!("i={i} t={} x={} y={}",(a&(1<<(29-i))!=0) as u8,Planar64::raw(x),Planar64::raw(y));
|
|
}
|
|
//don't forget the gain
|
|
(Planar64::raw(x),Planar64::raw(y))
|
|
*/
|
|
let (s,c)=(self.0 as f64*Self::ANGLE32_TO_FLOAT64_RADIANS).sin_cos();
|
|
(Planar64::raw((c*((1u64<<32) as f64)) as i64),Planar64::raw((s*((1u64<<32) as f64)) as i64))
|
|
}
|
|
}
|
|
impl Into<f32> for Angle32{
|
|
#[inline]
|
|
fn into(self)->f32{
|
|
(self.0 as f64*Self::ANGLE32_TO_FLOAT64_RADIANS) as f32
|
|
}
|
|
}
|
|
impl std::ops::Neg for Angle32{
|
|
type Output=Angle32;
|
|
#[inline]
|
|
fn neg(self)->Self::Output{
|
|
Angle32(self.0.wrapping_neg())
|
|
}
|
|
}
|
|
impl std::ops::Add<Angle32> for Angle32{
|
|
type Output=Angle32;
|
|
#[inline]
|
|
fn add(self,rhs:Self)->Self::Output {
|
|
Angle32(self.0.wrapping_add(rhs.0))
|
|
}
|
|
}
|
|
impl std::ops::Sub<Angle32> for Angle32{
|
|
type Output=Angle32;
|
|
#[inline]
|
|
fn sub(self,rhs:Self)->Self::Output {
|
|
Angle32(self.0.wrapping_sub(rhs.0))
|
|
}
|
|
}
|
|
impl std::ops::Mul<i32> for Angle32{
|
|
type Output=Angle32;
|
|
#[inline]
|
|
fn mul(self,rhs:i32)->Self::Output {
|
|
Angle32(self.0.wrapping_mul(rhs))
|
|
}
|
|
}
|
|
impl std::ops::Mul<Angle32> for Angle32{
|
|
type Output=Angle32;
|
|
#[inline]
|
|
fn mul(self,rhs:Self)->Self::Output {
|
|
Angle32(self.0.wrapping_mul(rhs.0))
|
|
}
|
|
}
|
|
#[test]
|
|
fn angle_sin_cos(){
|
|
fn close_enough(lhs:Planar64,rhs:Planar64)->bool{
|
|
(lhs-rhs).abs()<Planar64::EPSILON*4
|
|
}
|
|
fn test_angle(f:f64){
|
|
let a=Angle32((f/Angle32::ANGLE32_TO_FLOAT64_RADIANS) as i32);
|
|
println!("a={:#034b}",a.0);
|
|
let (c,s)=a.cos_sin();
|
|
let h=(s*s+c*c).sqrt();
|
|
println!("cordic s={} c={}",(s/h).divide(),(c/h).divide());
|
|
let (fs,fc)=f.sin_cos();
|
|
println!("float s={} c={}",fs,fc);
|
|
assert!(close_enough((c/h).divide().fix_1(),Planar64::raw((fc*((1u64<<32) as f64)) as i64)));
|
|
assert!(close_enough((s/h).divide().fix_1(),Planar64::raw((fs*((1u64<<32) as f64)) as i64)));
|
|
}
|
|
test_angle(1.0);
|
|
test_angle(std::f64::consts::PI/4.0);
|
|
test_angle(std::f64::consts::PI/8.0);
|
|
}
|
|
|
|
/* Unit type unused for now, may revive it for map files
|
|
///[-1.0,1.0] = [-2^30,2^30]
|
|
pub struct Unit32(i32);
|
|
impl Unit32{
|
|
#[inline]
|
|
pub fn as_planar64(&self) -> Planar64{
|
|
Planar64(4*(self.0 as i64))
|
|
}
|
|
}
|
|
const UNIT32_ONE_FLOAT64=((1<<30) as f64);
|
|
///[-1.0,1.0] = [-2^30,2^30]
|
|
pub struct Unit32Vec3(glam::IVec3);
|
|
impl TryFrom<[f32;3]> for Unit32Vec3{
|
|
type Error=Unit32TryFromFloatError;
|
|
fn try_from(value:[f32;3])->Result<Self,Self::Error>{
|
|
Ok(Self(glam::ivec3(
|
|
Unit32::try_from(Planar64::try_from(value[0])?)?.0,
|
|
Unit32::try_from(Planar64::try_from(value[1])?)?.0,
|
|
Unit32::try_from(Planar64::try_from(value[2])?)?.0,
|
|
)))
|
|
}
|
|
}
|
|
*/
|
|
|
|
pub type Planar64TryFromFloatError=fixed_wide::fixed::FixedFromFloatError;
|
|
pub type Planar64=fixed_wide::types::I32F32;
|
|
pub type Planar64Vec3=linear_ops::types::Vector3<Planar64>;
|
|
pub type Planar64Mat3=linear_ops::types::Matrix3<Planar64>;
|
|
pub mod vec3{
|
|
use super::*;
|
|
pub use linear_ops::types::Vector3;
|
|
pub const MIN:Planar64Vec3=Planar64Vec3::new([Planar64::MIN;3]);
|
|
pub const MAX:Planar64Vec3=Planar64Vec3::new([Planar64::MAX;3]);
|
|
pub const ZERO:Planar64Vec3=Planar64Vec3::new([Planar64::ZERO;3]);
|
|
pub const ZERO_2:linear_ops::types::Vector3<Fixed::<2,64>>=linear_ops::types::Vector3::new([Fixed::<2,64>::ZERO;3]);
|
|
pub const X:Planar64Vec3=Planar64Vec3::new([Planar64::ONE,Planar64::ZERO,Planar64::ZERO]);
|
|
pub const Y:Planar64Vec3=Planar64Vec3::new([Planar64::ZERO,Planar64::ONE,Planar64::ZERO]);
|
|
pub const Z:Planar64Vec3=Planar64Vec3::new([Planar64::ZERO,Planar64::ZERO,Planar64::ONE]);
|
|
pub const ONE:Planar64Vec3=Planar64Vec3::new([Planar64::ONE,Planar64::ONE,Planar64::ONE]);
|
|
pub const NEG_X:Planar64Vec3=Planar64Vec3::new([Planar64::NEG_ONE,Planar64::ZERO,Planar64::ZERO]);
|
|
pub const NEG_Y:Planar64Vec3=Planar64Vec3::new([Planar64::ZERO,Planar64::NEG_ONE,Planar64::ZERO]);
|
|
pub const NEG_Z:Planar64Vec3=Planar64Vec3::new([Planar64::ZERO,Planar64::ZERO,Planar64::NEG_ONE]);
|
|
pub const NEG_ONE:Planar64Vec3=Planar64Vec3::new([Planar64::NEG_ONE,Planar64::NEG_ONE,Planar64::NEG_ONE]);
|
|
#[inline]
|
|
pub const fn int(x:i32,y:i32,z:i32)->Planar64Vec3{
|
|
Planar64Vec3::new([Planar64::raw((x as i64)<<32),Planar64::raw((y as i64)<<32),Planar64::raw((z as i64)<<32)])
|
|
}
|
|
#[inline]
|
|
pub fn raw_array(array:[i64;3])->Planar64Vec3{
|
|
Planar64Vec3::new(array.map(Planar64::raw))
|
|
}
|
|
#[inline]
|
|
pub fn raw_xyz(x:i64,y:i64,z:i64)->Planar64Vec3{
|
|
Planar64Vec3::new([Planar64::raw(x),Planar64::raw(y),Planar64::raw(z)])
|
|
}
|
|
#[inline]
|
|
pub fn try_from_f32_array([x,y,z]:[f32;3])->Result<Planar64Vec3,Planar64TryFromFloatError>{
|
|
Ok(Planar64Vec3::new([
|
|
try_from_f32(x)?,
|
|
try_from_f32(y)?,
|
|
try_from_f32(z)?,
|
|
]))
|
|
}
|
|
}
|
|
|
|
#[inline]
|
|
pub fn int(value:i32)->Planar64{
|
|
Planar64::from(value)
|
|
}
|
|
#[inline]
|
|
pub fn try_from_f32(value:f32)->Result<Planar64,Planar64TryFromFloatError>{
|
|
let result:Result<Planar64,_>=value.try_into();
|
|
match result{
|
|
Ok(ok)=>Ok(ok),
|
|
Err(e)=>e.underflow_to_zero(),
|
|
}
|
|
}
|
|
pub mod mat3{
|
|
use super::*;
|
|
pub use linear_ops::types::Matrix3;
|
|
#[inline]
|
|
pub const fn identity()->Planar64Mat3{
|
|
Planar64Mat3::new([
|
|
[Planar64::ONE,Planar64::ZERO,Planar64::ZERO],
|
|
[Planar64::ZERO,Planar64::ONE,Planar64::ZERO],
|
|
[Planar64::ZERO,Planar64::ZERO,Planar64::ONE],
|
|
])
|
|
}
|
|
#[inline]
|
|
pub fn from_diagonal(diag:Planar64Vec3)->Planar64Mat3{
|
|
Planar64Mat3::new([
|
|
[diag.x,Planar64::ZERO,Planar64::ZERO],
|
|
[Planar64::ZERO,diag.y,Planar64::ZERO],
|
|
[Planar64::ZERO,Planar64::ZERO,diag.z],
|
|
])
|
|
}
|
|
#[inline]
|
|
pub fn from_rotation_yx(x:Angle32,y:Angle32)->Planar64Mat3{
|
|
let (xc,xs)=x.cos_sin();
|
|
let (yc,ys)=y.cos_sin();
|
|
Planar64Mat3::from_cols([
|
|
Planar64Vec3::new([xc,Planar64::ZERO,-xs]),
|
|
Planar64Vec3::new([(xs*ys).fix_1(),yc,(xc*ys).fix_1()]),
|
|
Planar64Vec3::new([(xs*yc).fix_1(),-ys,(xc*yc).fix_1()]),
|
|
])
|
|
}
|
|
#[inline]
|
|
pub fn from_rotation_y(y:Angle32)->Planar64Mat3{
|
|
let (c,s)=y.cos_sin();
|
|
Planar64Mat3::from_cols([
|
|
Planar64Vec3::new([c,Planar64::ZERO,-s]),
|
|
vec3::Y,
|
|
Planar64Vec3::new([s,Planar64::ZERO,c]),
|
|
])
|
|
}
|
|
#[inline]
|
|
pub fn try_from_f32_array_2d([x_axis,y_axis,z_axis]:[[f32;3];3])->Result<Planar64Mat3,Planar64TryFromFloatError>{
|
|
Ok(Planar64Mat3::new([
|
|
vec3::try_from_f32_array(x_axis)?.to_array(),
|
|
vec3::try_from_f32_array(y_axis)?.to_array(),
|
|
vec3::try_from_f32_array(z_axis)?.to_array(),
|
|
]))
|
|
}
|
|
}
|
|
|
|
#[derive(Clone,Copy,Default,Hash,Eq,PartialEq)]
|
|
pub struct Planar64Affine3{
|
|
pub matrix3:Planar64Mat3,//includes scale above 1
|
|
pub translation:Planar64Vec3,
|
|
}
|
|
impl Planar64Affine3{
|
|
#[inline]
|
|
pub const fn new(matrix3:Planar64Mat3,translation:Planar64Vec3)->Self{
|
|
Self{matrix3,translation}
|
|
}
|
|
#[inline]
|
|
pub fn transform_point3(&self,point:Planar64Vec3)->vec3::Vector3<Fixed<2,64>>{
|
|
self.translation.fix_2()+self.matrix3*point
|
|
}
|
|
}
|
|
impl Into<glam::Mat4> for Planar64Affine3{
|
|
#[inline]
|
|
fn into(self)->glam::Mat4{
|
|
let matrix3=self.matrix3.to_array().map(|row|row.map(Into::<f32>::into));
|
|
let translation=self.translation.to_array().map(Into::<f32>::into);
|
|
glam::Mat4::from_cols_array(&[
|
|
matrix3[0][0],matrix3[0][1],matrix3[0][2],0.0,
|
|
matrix3[1][0],matrix3[1][1],matrix3[1][2],0.0,
|
|
matrix3[2][0],matrix3[2][1],matrix3[2][2],0.0,
|
|
translation[0],translation[1],translation[2],1.0
|
|
])
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn test_sqrt(){
|
|
let r=int(400);
|
|
assert_eq!(r,Planar64::raw(1717986918400));
|
|
let s=r.sqrt();
|
|
assert_eq!(s,Planar64::raw(85899345920));
|
|
}
|