(*
Quadric.sml

Seth Porter <symmetry>

Implements Garland's quadric error metric,
as described in his thesis.

Currently hard-wired for 3D case; needs to be generalized


*)

(*
structure Quadric : ERROR_METRIC =
*)
functor Quadric(structure Matrix : MATRIX
		structure Geometry : GEOMETRY 
		    sharing type Geometry.Number.t = Matrix.scalar)
    :> ERROR_METRIC where Geometry=Geometry =
struct
    structure Geometry = Geometry
    exception Strange
    type number = Geometry.Number.t

	(* convenience alias *)
    structure Number = Geometry.Number

    fun abs x =
	let 
	    open Number
	in
	    if x <= zero then
		~x
	    else x
	end
    
    val tol = Number.fromReal(1E~12)
	
    fun FEQ(a, b, epsilon) =
	let
	    open Number
	    val diff = abs(a-b)
	in
	    diff <= epsilon
	end


    (* Follows MxQuadric3 *)
    (* But need to re-cast it in matrix terms for 
     generalization across vector-space *)
    type metric = { a2 : number, ab : number,
	      ac : number, ad : number,
	      b2 : number, bc : number,
	      bd : number, c2 : number,
	      cd : number, d2 : number,
	      r : number }

	(* local alias *)
    type t = metric
    val zero =
	let
	    val z = Number.zero
	in
	    {a2=z, ab=z, ac=z, ad=z,
	     b2=z, bc=z, bd=z,
	     c2=z, cd=z,
	     d2=z,
	     r=z} : t
	end

    (* MxQuadric3 operator *= *)
    fun scale({a2, ab, ac, ad, b2, bc, bd, c2, cd, d2, r}, s) =
	let
	    open Number
	in
	    {a2=a2*s, ab=ab*s, ac=ac*s, ad=ad*s, 
	     b2=b2*s, bc=bc*s, bd=bd*s, 
	     c2=c2*s, cd=cd*s, 
	     d2=d2*s,
	     r=r}
	end
    fun plus({a2, ab, ac, ad, b2, bc, bd, c2, cd, d2, r} : t,
	    {a2=a2', ab=ab', ac=ac', ad=ad', b2=b2', bc=bc', bd=bd', 
	     c2=c2', cd=cd', d2=d2', r=r'} : t)=
	let
	    open Number
	in
	    {a2=a2+a2', ab=ab+ab', ac=ac+ac', ad=ad+ad',
	     b2=b2+b2', bc=bc+bc', bd=bd+bd',
	     c2=c2+c2', cd=cd+cd',
	     d2=d2+d2',
	     r=r+r'}
	end
    
    fun tensor({a2, ab, ac, ad, b2, bc, bd, c2, cd, d2, ...} : t) =
	Matrix.fromList(
			[[a2, ab, ac],
			 [ab, b2, bc],
			 [ac, bc, c2]])
	
    fun homogeneous({a2, ab, ac, ad, b2, bc, bd, c2, cd, d2, ...} : t) =
	Matrix.fromList(
			[[a2, ab, ac, ad],
			 [ab, b2, bc, bd],
			 [ac, bc, c2, cd],
			 [ad, bd, cd, d2]])
    fun vector({ad, bd, cd, ...} : t) =
	Matrix.fromList [[ad], [bd], [cd]]
	
    fun invDet(Q : t) =
	let
	    val mat = tensor(Q)
	    val (flag, (inv, det)) = (true, Matrix.invDet(mat))
		handle Matrix.notInvertable => (false, (mat, Number.zero))
	in
	    (* if singular or very close *)
	    if (not flag) orelse FEQ(det, Number.zero, tol) then
		NONE
	    else
		SOME(inv)
	end
	
(*	fun optimalLine(Q : t, v1, v2) =
	    let
		val d = Vec.-(v1, v2)
		val A = tensor Q
		val Av2 = Matrix.*(A, v2)
		val Ad = Matrix.*(A, d)
		val denom = 2.0 * (Matrix.*(d, Ad))
	    in
		if FEQ(denom, 0.0, tol) then
		    NONE
		else
	*)	    



    fun init'(a, b, c, d, area) : t=
	let
	    open Number
	in
	    { a2 = a*a, ab = a*b,
	     ac = a*c, ad = a*d,
	     b2 = b*b, bc = b*c,
	     bd = b*d, c2 = c*c,
	     cd = c*d, d2 = d*d,
	     r = area }
	end

(*
 Initialization code; other strategies
 are possible, so this may eventually
 be factored into another structure.
 *)
    structure Vec = Geometry.Point.Vec


    fun triangleRawNormal(v1, v2, v3) =
	(* MxGeom3d.cxx: triangle_raw_normal *)
	let
	    open Geometry.Point
	    val a = v2 - v1
	    val b = v3 - v1
	in
	    Vec.cross #[a, b]
	end

    fun unitize v =
	let
	    val n = Vec.norm v
	    val one = Number.fromReal 1.0
	in
	    if not(FEQ(n, Number.zero, tol)) andalso not(FEQ(n,one,tol)) then
		Vec.scale(v, Number./ (one, n))
	    else
		v
	end

    fun triangleNormal(v1, v2, v3) =
	(* MxGeom3d.cxx: triangle_normal *)
	unitize(triangleRawNormal(v1, v2, v3))

    local
	fun trianglePlane'(f, (v1, v2, v3)) =
	    let
		val n = f(v1, v2, v3)
		open Number
		val w = ~(Vec.dot(n, Geometry.Point.toVec v1))
		val #[x, y, z] = Vec.toSeq n
	    in
		(* pseudo 4D vec *)
		(x, y, z, w)
	    end
    in
	(* MxGeom3d.cxx: triangle_plane *)
	fun trianglePlane(v) =
	    trianglePlane'(triangleNormal, v)
	(* MxGeom3d.cxx: triangle_raw_plane *)
	fun triangleRawPlane(v) =
	    trianglePlane'(triangleRawNormal, v)
    end

    fun faceArea(v1, v2, v3) =
	(* MxBlockModel::compute_face_area *)
	(* Garland uses MxBlockModel::compute_face_normal,
	   but same functionality *)
	let
	    val n = triangleRawNormal(v1, v2, v3)
	    open Number
	    val half = Number.fromReal 0.5
	in
	    half * (Vec.norm n)
	end

    (* Body of loop in MxQSlim::collect_quadrics *)
    fun makeQuadric(v1, v2, v3) = 
	let
	    (* Make this user-controllable *)
	    val policy_RAWNORMALS = false
	    val policy_WEIGHT_ANGLE = false


	    (* Note that we do a fair amount of
	       redundant calculation here (as
	       does Garland) -- calculate normal
	       and norm twice *)

	    val (x, y, z, w) =
		if policy_RAWNORMALS then
		    triangleRawPlane(v1, v2, v3)
		else
		    trianglePlane(v1, v2, v3)
	    val area = faceArea(v1, v2, v3)
	    val q = init'(x, y, z, w, area)
	in
	    if policy_WEIGHT_ANGLE then
		raise Strange (* really NYI *)
	    else
		scale(q, area)
	end

    fun init(vertices) =
	let
	    val #[v1, v2, v3] = vertices
	in
	    makeQuadric(v1, v2, v3)
	end

(*
 End of initialization code
 *)



    fun optimal(Q : t) : Geometry.Point.point option =
	(* return -inv(M)*b *)
	case invDet(Q) of
	    NONE =>	NONE
	  | SOME(inv) => 
		let
		    val b = vector Q
		    val neg_opt = Matrix.*(inv, b)
		    val opt = Matrix.scale(neg_opt, Number.fromReal ~1.0)
		in
		    (* There really should be a better way to
		     turn a column matrix into a vector... *)
		    (* Guy says to use tabulate *)
		    SOME(Geometry.Point.fromSeq (Sequence.fromList (
				 List.concat(Matrix.toList opt))))
		end
		
    fun evaluate({a2, ab, ac, ad, b2, bc, bd, c2, cd, d2, ...} : t, v) =
	let
	    open Number
	    val #[x, y, z] = Geometry.Point.toSeq v
	    val two = fromReal(2.0)
	in
	    x*x*a2 + two*x*y*ab + two*x*z*ac + two*x*ad
	           + y*y*b2     + two*y*z*bc + two*y*bd
	                        + z*z*c2     + two*z*cd
           	                             + d2
	end
    fun toString({a2, ab, ac, ad, b2, bc, bd, c2, cd, d2, r} : t) =
	let
	    fun ro r  = (Number.toString r) ^ "  "
	in
	    (ro a2) ^ (ro ab) ^ (ro ac) ^ (ro ad) ^ (ro b2) ^ (ro bc) ^
	    (ro bd) ^ (ro c2) ^ (ro cd) ^ (ro d2) ^ (ro r)
	end
    
end