mirror of
https://github.com/allaunthefox/Research-Stack.git
synced 2026-07-31 03:05:21 +00:00
117 lines
3.2 KiB
Text
117 lines
3.2 KiB
Text
import Semantics.PhysicsScalar
|
|
|
|
namespace Semantics.PhysicsEuclidean
|
|
|
|
open Semantics.PhysicsScalar
|
|
|
|
abbrev Q16_16 := PhysicsScalar.Q16_16
|
|
|
|
structure PhysicsEuclidean (n : Nat) where
|
|
coords : Fin n → Q16_16
|
|
|
|
namespace PhysicsEuclidean
|
|
|
|
def zero (n : Nat) : PhysicsEuclidean n :=
|
|
{ coords := fun _ => Q16_16.zero }
|
|
|
|
instance {n : Nat} : Inhabited (PhysicsEuclidean n) where
|
|
default := zero n
|
|
|
|
def component (vector : PhysicsEuclidean n) (index : Fin n) : Q16_16 :=
|
|
vector.coords index
|
|
|
|
def map (vector : PhysicsEuclidean n) (f : Q16_16 → Q16_16) : PhysicsEuclidean n :=
|
|
{ coords := fun index => f (vector.coords index) }
|
|
|
|
|
|
def zipWith
|
|
(left right : PhysicsEuclidean n)
|
|
(f : Q16_16 → Q16_16 → Q16_16) : PhysicsEuclidean n :=
|
|
{ coords := fun index => f (left.coords index) (right.coords index) }
|
|
|
|
|
|
def add (left right : PhysicsEuclidean n) : PhysicsEuclidean n :=
|
|
zipWith left right Q16_16.add
|
|
|
|
|
|
def sub (left right : PhysicsEuclidean n) : PhysicsEuclidean n :=
|
|
zipWith left right Q16_16.sub
|
|
|
|
|
|
def componentwiseMin (left right : PhysicsEuclidean n) : PhysicsEuclidean n :=
|
|
zipWith left right Q16_16.min
|
|
|
|
|
|
def componentwiseMax (left right : PhysicsEuclidean n) : PhysicsEuclidean n :=
|
|
zipWith left right Q16_16.max
|
|
|
|
|
|
def scale (scalar : Q16_16) (vector : PhysicsEuclidean n) : PhysicsEuclidean n :=
|
|
map vector (fun value => Q16_16.mul scalar value)
|
|
|
|
|
|
def hadamard (left right : PhysicsEuclidean n) : PhysicsEuclidean n :=
|
|
zipWith left right Q16_16.mul
|
|
|
|
|
|
def dotAccumulate (index : Nat) (left right : PhysicsEuclidean n) (acc : Q16_16) : Q16_16 :=
|
|
match h : index with
|
|
| 0 => acc
|
|
| Nat.succ prev =>
|
|
if hlt : prev < n then
|
|
let finIndex : Fin n := ⟨prev, hlt⟩
|
|
let product := Q16_16.mul (left.coords finIndex) (right.coords finIndex)
|
|
dotAccumulate prev left right (Q16_16.add acc product)
|
|
else
|
|
acc
|
|
|
|
|
|
def dot (left right : PhysicsEuclidean n) : Q16_16 :=
|
|
dotAccumulate n left right Q16_16.zero
|
|
|
|
|
|
def l1Accumulate (index : Nat) (vector : PhysicsEuclidean n) (acc : Q16_16) : Q16_16 :=
|
|
match h : index with
|
|
| 0 => acc
|
|
| Nat.succ prev =>
|
|
if hlt : prev < n then
|
|
let finIndex : Fin n := ⟨prev, hlt⟩
|
|
l1Accumulate prev vector (Q16_16.add acc (vector.coords finIndex))
|
|
else
|
|
acc
|
|
|
|
|
|
def l1Norm (vector : PhysicsEuclidean n) : Q16_16 :=
|
|
l1Accumulate n vector Q16_16.zero
|
|
|
|
|
|
def approxNorm (vector : PhysicsEuclidean n) : Q16_16 :=
|
|
l1Norm vector
|
|
|
|
|
|
def distanceApprox (left right : PhysicsEuclidean n) : Q16_16 :=
|
|
approxNorm (sub (componentwiseMax left right) (componentwiseMin left right))
|
|
|
|
|
|
def clampComponents
|
|
(vector lower upper : PhysicsEuclidean n) : PhysicsEuclidean n :=
|
|
{ coords := fun index => Q16_16.clamp (vector.coords index) (lower.coords index) (upper.coords index) }
|
|
|
|
|
|
def withComponent
|
|
(vector : PhysicsEuclidean n)
|
|
(index : Fin n)
|
|
(value : Q16_16) : PhysicsEuclidean n :=
|
|
{ coords := fun probe => if probe = index then value else vector.coords probe }
|
|
|
|
|
|
def sumComponents (vector : PhysicsEuclidean n) : Q16_16 :=
|
|
l1Norm vector
|
|
|
|
end PhysicsEuclidean
|
|
|
|
abbrev PhysicsVec2 := PhysicsEuclidean 2
|
|
abbrev PhysicsVec3 := PhysicsEuclidean 3
|
|
abbrev PhysicsVec4 := PhysicsEuclidean 4
|
|
|
|
end Semantics.PhysicsEuclidean
|