diff --git a/src/physics.rs b/src/physics.rs index 4a78d37..fcaf958 100644 --- a/src/physics.rs +++ b/src/physics.rs @@ -763,23 +763,31 @@ impl TouchingState{ a } fn constrain_velocity(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,velocity:&mut Planar64Vec3){ - //TODO: trey push solve - for contact in &self.contacts{ + let contacts=self.contacts.iter().map(|contact|{ let n=contact_normal(models,hitbox_mesh,contact); - let d=n.dot128(*velocity); - if d<0{ - *velocity-=n*Planar64::raw(((d<<32)/n.dot128(n)) as i64); + crate::push_solve::Contact{ + position:Planar64Vec3::ZERO, + velocity:n, + normal:n, } + }).collect(); + match crate::push_solve::push_solve(&contacts,*velocity){ + Some(new_velocity)=>*velocity=new_velocity, + None=>println!("Algorithm silently failing :)"), } } fn constrain_acceleration(&self,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,acceleration:&mut Planar64Vec3){ - //TODO: trey push solve - for contact in &self.contacts{ + let contacts=self.contacts.iter().map(|contact|{ let n=contact_normal(models,hitbox_mesh,contact); - let d=n.dot128(*acceleration); - if d<0{ - *acceleration-=n*Planar64::raw(((d<<32)/n.dot128(n)) as i64); + crate::push_solve::Contact{ + position:Planar64Vec3::ZERO, + velocity:n, + normal:n, } + }).collect(); + match crate::push_solve::push_solve(&contacts,*acceleration){ + Some(new_acceleration)=>*acceleration=new_acceleration, + None=>println!("Algorithm silently failing :)"), } } fn predict_collision_end(&self,collector:&mut instruction::InstructionCollector,models:&PhysicsModels,hitbox_mesh:&HitboxMesh,body:&Body,time:Time){