geom-vec.stanza

defpackage geom/vec :
  import core
  import math
  import utils/rnd
  import geom/geom
  import geom/utils

defn round (x:Int) : x
defn sqrt (x:Int) : to-int(round(sqrt(to-float(x))))

val @doc-vx = "2/3 DIM VECTORS -- ponts and dimensions"

#for (Prim in [Int Float Double]
      prim in [int float double]
      PrimVec in [Vec3i Vec3f Vec3]
      PrimVecName in ["Vec3i" "Vec3f" "Vec3"]
      PrimVec2 in [Vec2i Vec2f Vec2]
      eset in [e3 e3 e3]
      xset in [x3 x3 x3]
      yset in [y3 y3 y3]
      zset in [z3 z3 z3]
      prim-fill in [Vec3-fill Vec3-fill Vec3-fill]
      prim-unit in [Vec3i-unit Vec3f-unit Vec3-unit]
      x0 in [0 0.0f 0.0]
      x1 in [1 1.0f 1.0]) :

  public lostanza deftype PrimVec <: Array<Prim> & Hashable & Geom :
    x: prim
    y: prim
    z: prim

  public lostanza defn PrimVec (x:ref<Prim>, y:ref<Prim>, z:ref<Prim>) -> ref<PrimVec> :
    return new PrimVec{x.value, y.value, z.value}

  public lostanza defn x (v:ref<PrimVec>) -> ref<Prim> :
    return new Prim{v.x}

  public lostanza defn y (v:ref<PrimVec>) -> ref<Prim> :
    return new Prim{v.y}

  public lostanza defn z (v:ref<PrimVec>) -> ref<Prim> :
    return new Prim{v.z}

  public defn PrimVec (x:Prim, y:Prim) -> PrimVec :
    PrimVec(x, y, x0)

  public defn prim-fill (a:Prim) : PrimVec(a, a, a)

  public defn prim-unit (i:Int) :
    if i == 0 :      PrimVec(x1, x0, x0)
    else if i == 1 : PrimVec(x0, x1, x0)
    else :           PrimVec(x0, x0, x1)

  public defn eset (i:Int, a:Prim) -> PrimVec : a * prim-unit(i)
  public defn xset (a:Prim) -> PrimVec : PrimVec(a, x0, x0)
  public defn yset (a:Prim) -> PrimVec : PrimVec(x0, a, x0)
  public defn zset (a:Prim) -> PrimVec : PrimVec(x0, x0, a)

  public defn set-elt (v:PrimVec, i:Int, a:Prim) -> PrimVec :
    PrimVec(a when i == 0 else v[0], a when i == 1 else v[1], a when i == 2 else v[2])

  lostanza defmethod get (v:ref<PrimVec>, i:ref<Int>) -> ref<Prim> : 
    ensure-index-in-bounds(i, new Int{3})
    return new Prim{addr!(v.x)[i.value]}

  defmethod length (a:PrimVec) -> Int : 3

  public defn area (a:PrimVec) -> Prim :
    x(a) * y(a)

  public defn volume (a:PrimVec) -> Prim :
    x(a) * y(a) * z(a)

  defmethod equal? (v1:PrimVec, v2:PrimVec) :
    x(v1) == x(v2) and y(v1) == y(v2) and z(v1) == z(v2) 

  defmethod hash (v:PrimVec) :
    (7 * ((7 * hash(x(v))) + hash(y(v)))) + hash(z(v))

  public defn xy (p:PrimVec) -> PrimVec2 :
    PrimVec2(x(p), y(p))

  public defn xyz (p:PrimVec) -> PrimVec : p

  public defn xyz (p:Prim) -> PrimVec : PrimVec(p, p, p)

  defmethod print (o:OutputStream, v:PrimVec) :
    print(o, "%_(%~, %~, %~)" % [PrimVecName, x(v), y(v), z(v)])

  public defn magnitude (a:PrimVec) -> Prim :
    sqrt(dot(a, a))

  public defn distance (v0:PrimVec, v1:PrimVec) -> Prim :
    magnitude(v1 - v0)

  public defn normalize (a:PrimVec) -> PrimVec :
    a / magnitude(a)

  public defn dot (a:PrimVec, b:PrimVec) -> Prim :
    x(a) * x(b) + y(a) * y(b) + z(a) * z(b)

  public defn min (a:PrimVec) -> Prim :
    min(x(a), min(y(a), z(a)))

  public defn max (a:PrimVec) -> Prim :
    max(x(a), max(y(a), z(a)))

  public defn min (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(min(x(a), x(b)), min(y(a), y(b)), min(z(a), z(b)))

  public defn max (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(max(x(a), x(b)), max(y(a), y(b)), max(z(a), z(b)))

  public defn times (a:Prim, b:PrimVec) -> PrimVec :
    PrimVec(a * x(b), a * y(b), a * z(b))

  public defn times (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(x(a) * x(b), y(a) * y(b), z(a) * z(b))

  public defn divide (a:PrimVec, b:Prim) -> PrimVec :
    PrimVec(x(a) / b, y(a) / b, z(a) / b)

  public defn divide (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(x(a) / x(b), y(a) / y(b), z(a) / z(b))

  public defn modulo (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(y(a) * z(b) - z(a) * y(b),
            z(a) * x(b) - x(a) * z(b),
            x(a) * y(b) - y(a) * x(b))

  public defn plus (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(x(a) + x(b), y(a) + y(b), z(a) + z(b))

  public defn minus (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(x(a) - x(b), y(a) - y(b), z(a) - z(b))

  public defn negate (a:PrimVec) -> PrimVec :
    PrimVec((- x(a)), (- y(a)), (- z(a)))

  public defn abs (a:PrimVec) -> PrimVec :
    PrimVec(abs(x(a)), abs(y(a)), abs(z(a)))

  public defn map (f:Prim -> Prim, v:PrimVec) -> PrimVec :
    PrimVec(f(x(v)), f(y(v)), f(z(v)))

  public defn round (p:PrimVec) -> PrimVec :
    PrimVec(round(x(p)), round(y(p)), round(z(p)))

  public defn sorted-indices (v:PrimVec) -> Vec3i :
    if x(v) < y(v) :
      if x(v) < z(v) :
        Vec3i(0, 1, 2) when y(v) < z(v) else Vec3i(0, 2, 1)
      else :
        Vec3i(2, 0, 1)
    else :
      if y(v) < z(v) :
        Vec3i(1, 0, 2) when x(v) < z(v) else Vec3i(1, 2, 0)
      else :
        Vec3i(2, 1, 0)

  public defn min-idx (v:PrimVec) -> Int :
    sorted-indices(v)[0]

  public defn mid-idx (v:PrimVec) -> Int :
    sorted-indices(v)[1]

  public defn max-idx (v:PrimVec) -> Int :
    sorted-indices(v)[2]

  public defn rnd (lo:PrimVec, hi:PrimVec) -> PrimVec :
    PrimVec(rnd(x(lo), x(hi)), rnd(y(lo), y(hi)), rnd(z(lo), z(hi)))


public defn indices (dims:Vec3i) -> Seq<Vec3i> :
  generate<Vec3i> :
    for k in 0 to z(dims) do :
      for j in 0 to y(dims) do :
        for i in 0 to x(dims) do :
          yield(Vec3i(i, j, k))

#for (Prim in [Float Double]
      PrimVec in [Vec3f Vec3]) :
  public defn angle (a:PrimVec, b:PrimVec) -> Prim :
    acos(dot(a, b) / (magnitude(a) * magnitude(b)))

;;; V2x

#for (Prim in [Int Float Double]
      prim in [int float double]
      PrimVec in [Vec2i Vec2f Vec2]
      PrimVecName in ["Vec2i" "Vec2f" "Vec2"]
      PrimVec3 in [Vec3i Vec3f Vec3]
      eset in [e2 e2 e2]
      xset in [x2 x2 x2]
      yset in [y2 y2 y2]
      prim-fill in [Vec2-fill Vec2-fill Vec2-fill]
      prim-unit in [Vec2i-unit Vec2f-unit Vec2-unit]
      x0 in [0 0.0f 0.0]
      x1 in [1 1.0f 1.0]) :

  public lostanza deftype PrimVec <: Array<Prim> & Hashable & Geom :
    x: prim
    y: prim

  public lostanza defn PrimVec (x:ref<Prim>, y:ref<Prim>) -> ref<PrimVec> :
    return new PrimVec{x.value, y.value}

  public lostanza defn x (v:ref<PrimVec>) -> ref<Prim> :
    return new Prim{v.x}

  public lostanza defn y (v:ref<PrimVec>) -> ref<Prim> :
    return new Prim{v.y}

  public defn prim-fill (a:Prim) : PrimVec(a, a)

  public defn prim-unit (i:Int) :
    if i == 0 : PrimVec(x1, x0)
    else :      PrimVec(x0, x1)

  public defn eset (i:Int, a:Prim) -> PrimVec : a * prim-unit(i)
  public defn xset (a:Prim) -> PrimVec : PrimVec(a, x0)
  public defn yset (a:Prim) -> PrimVec : PrimVec(x0, a)

  public defn set-elt (v:PrimVec, i:Int, a:Prim) -> PrimVec :
    PrimVec(a when i == 0 else v[0], a when i == 1 else v[1])

  lostanza defmethod get (v:ref<PrimVec>, i:ref<Int>) -> ref<Prim> : 
    ensure-index-in-bounds(i, new Int{2})
    return new Prim{addr!(v.x)[i.value]}

  defmethod length (a:PrimVec) -> Int : 2

  public defn area (a:PrimVec) -> Prim :
    x(a) * y(a)

  public defn volume (a:PrimVec) -> Prim :
    x0

  defmethod equal? (v1:PrimVec, v2:PrimVec) :
    x(v1) == x(v2) and y(v1) == y(v2)

  defmethod hash (v:PrimVec) :
    ((7 * hash(x(v))) + hash(y(v)))

  public defn xy (p:PrimVec) -> PrimVec : p

  public defn xyz (p:PrimVec) -> PrimVec3 : PrimVec3(x(p), y(p), x0)

  public defn xy (p:Prim) -> PrimVec : PrimVec(p, p)

  defmethod print (o:OutputStream, v:PrimVec) :
    print(o, "%_(%~, %~)" % [PrimVecName, x(v), y(v)])

  public defn sqr-magnitude (a:PrimVec) -> Prim :
    dot(a, a)

  public defn magnitude (a:PrimVec) -> Prim :
    sqrt(dot(a, a))

  public defn distance (v0:PrimVec, v1:PrimVec) -> Prim :
    magnitude(v1 - v0)

  public defn normalize (a:PrimVec) -> PrimVec :
    a / magnitude(a)

  public defn dot (a:PrimVec, b:PrimVec) -> Prim :
    x(a) * x(b) + y(a) * y(b)

  public defn min (a:PrimVec) -> Prim :
    min(x(a), y(a))

  public defn max (a:PrimVec) -> Prim :
    max(x(a), y(a))

  public defn min (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(min(x(a), x(b)), min(y(a), y(b)))

  public defn max (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(max(x(a), x(b)), max(y(a), y(b)))

  public defn times (a:Prim, b:PrimVec) -> PrimVec :
    PrimVec(a * x(b), a * y(b))

  public defn times (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(x(a) * x(b), y(a) * y(b))

  public defn divide (a:PrimVec, b:Prim) -> PrimVec :
    PrimVec(x(a) / b, y(a) / b)

  public defn divide (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(x(a) / x(b), y(a) / y(b))

  public defn plus (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(x(a) + x(b), y(a) + y(b))

  public defn minus (a:PrimVec, b:PrimVec) -> PrimVec :
    PrimVec(x(a) - x(b), y(a) - y(b))

  public defn negate (a:PrimVec) -> PrimVec :
    PrimVec((- x(a)), (- y(a)))

  public defn modulo (a:PrimVec, b:PrimVec) -> Prim :
    x(a) * y(b) - y(a) * x(b)

  public defn abs (a:PrimVec) -> PrimVec :
    PrimVec(abs(x(a)), abs(y(a)))

  public defn map (f:Prim -> Prim, v:PrimVec) -> PrimVec :
    PrimVec(f(x(v)), f(y(v)))

  public defn zip (f:(Prim, Prim) -> Prim, v0:PrimVec, v1:PrimVec):
    PrimVec(f(x(v0), x(v1)), f(y(v0), y(v1)))

  public defn round (p:PrimVec) -> PrimVec :
    PrimVec(round(x(p)), round(y(p)))

  public defn sorted-indices (v:PrimVec) -> Vec2i :
    Vec2i(0, 1) when x(v) < y(v) else Vec2i(1, 0)

  public defn min-idx (v:PrimVec) -> Int :
    0 when x(v) < y(v) else 1

  public defn max-idx (v:PrimVec) -> Int :
    1 when x(v) < y(v) else 0

  public defn rnd (lo:PrimVec, hi:PrimVec) -> PrimVec :
    PrimVec(rnd(x(lo), x(hi)), rnd(y(lo), y(hi)))


#for (Prim in [Float Double]
      PrimVec in [Vec2f Vec2]) :
  public defn angle (a:PrimVec, b:PrimVec) -> Prim :
    acos(dot(a, b) / (magnitude(a) * magnitude(b)))

public defn to-vec3 (v:Vec3f) -> Vec3 :
  Vec3(to-double(x(v)), to-double(y(v)), to-double(z(v)))

public defn to-vec3i (v:Vec3f) -> Vec3i :
  Vec3i(to-int(x(v)), to-int(y(v)), to-int(z(v)))

public defn to-vec3i (v:Vec3) -> Vec3i :
  Vec3i(to-int(x(v)), to-int(y(v)), to-int(z(v)))

public defn to-vec3f (v:Vec3f) -> Vec3f :
  Vec3f(to-float(x(v)), to-float(y(v)), to-float(z(v)))

public defn to-vec3 (v:Vec3i) -> Vec3 :
  Vec3(to-double(x(v)), to-double(y(v)), to-double(z(v)))

public defn to-vec2i (v:Vec3f) -> Vec2i :
  Vec2i(to-int(x(v)), to-int(y(v)))

public defn to-vec2i (v:Vec2) -> Vec2i :
  Vec2i(to-int(x(v)), to-int(y(v)))

public defn to-vec2f (v:Vec2i) -> Vec2f :
  Vec2f(to-float(x(v)), to-float(y(v)))

public defn to-vec2d (v:Vec2i) -> Vec2 :
  Vec2(to-double(x(v)), to-double(y(v)))

public defn Vec2f (x:Double, y:Double) -> Vec2f :
  Vec2f(to-float(x), to-float(y))

public defn bisection-normal (a:Vec3, b:Vec3) -> Vec3 :
  val n = 
    if magnitude(a + b) < EPS :
      a
    else :
      (a + b) % (a % b)
  normalize(n)

public defn normal (a:Vec3, b:Vec3) -> Vec3 :
  val n = 
    if magnitude(a + b) < EPS :
      a
    else :
      (a % b)
  normalize(n)

;;; Vec4

;; public defstruct Vec4 <: Array<Float> :
;;   x: Float
;;   y: Float
;;   z: Float
;;   a: Float

val @doc-v4f = "4 DIM VECTORS -- color"

public lostanza deftype Vec4 <: Array<Double> & Geom & Hashable :
  x: double
  y: double
  z: double
  a: double

public lostanza defn Vec4 (x:ref<Double>, y:ref<Double>, z:ref<Double>, a:ref<Double>) -> ref<Vec4> :
  return new Vec4{x.value, y.value, z.value, a.value}

public lostanza defn x (v:ref<Vec4>) -> ref<Double> :
  return new Double{v.x}

public lostanza defn y (v:ref<Vec4>) -> ref<Double> :
  return new Double{v.y}

public lostanza defn z (v:ref<Vec4>) -> ref<Double> :
  return new Double{v.z}

public lostanza defn a (v:ref<Vec4>) -> ref<Double> :
  return new Double{v.a}

public defn Vec4 (x:Double, y:Double, z:Double) -> Vec4 :
  Vec4(x, y, z, 0.0)

public lostanza defmethod get (v:ref<Vec4>, i:ref<Int>) -> ref<Double> :
  ensure-index-in-bounds(i, new Int{3})
  return new Double{addr!(v.x)[i.value]}

public defmethod length (a:Vec4) -> Int : 4

defmethod equal? (v1:Vec4, v2:Vec4) :
  x(v1) == x(v2) and y(v1) == y(v2) and z(v1) == z(v2) and a(v1) == a(v2) 

defmethod hash (v:Vec4) :
  (7 * ((7 * ((7 * hash(x(v))) + hash(y(v)))) + hash(z(v)))) + hash(a(v))

defmethod print (o:OutputStream, v:Vec4) :
  print(o, "Vec4(%~, %~, %~, %~)" % [x(v), y(v), z(v), z(v)])

public val RED   = Vec4(1.0, 0.0, 0.0, 1.0)
public val GREEN = Vec4(0.0, 1.0, 0.0, 1.0)
public val BLUE  = Vec4(0.0, 0.0, 1.0, 1.0)
public val BLACK  = Vec4(0.0, 0.0, 0.0, 1.0)
public val WHITE  = Vec4(1.0, 1.0, 1.0, 1.0)

public defn area* (dims:Vec3f) -> Float : dims[max-idx(dims)] * dims[mid-idx(dims)]
public defn area* (dims:Vec3) -> Double : dims[max-idx(dims)] * dims[mid-idx(dims)]