From 81389a911a05af21d5b404400cbfc398a243a5a1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= <developer@crozet.re>
Date: Mon, 22 Jul 2013 23:20:03 +0200
Subject: [PATCH] Add utility methods.

Added look_at for 3d rotation matrix and 3d transform.
Rotation matrices constructors are now the static methods Rotmat::from_angle,
Rotmat::from_axis_angle.
---
 src/adaptors/rotmat.rs    | 89 ++++++++++++++++++++++++---------------
 src/adaptors/transform.rs | 15 ++++++-
 2 files changed, 68 insertions(+), 36 deletions(-)

diff --git a/src/adaptors/rotmat.rs b/src/adaptors/rotmat.rs
index 955ebe14..6d678639 100644
--- a/src/adaptors/rotmat.rs
+++ b/src/adaptors/rotmat.rs
@@ -3,6 +3,7 @@ use std::rand::{Rand, Rng, RngUtil};
 use std::cmp::ApproxEq;
 use traits::division_ring::DivisionRing;
 use traits::rlmul::{RMul, LMul};
+use traits::cross::Cross;
 use traits::dim::Dim;
 use traits::inv::Inv;
 use traits::transpose::Transpose;
@@ -25,50 +26,68 @@ impl<M: Clone> Rotmat<M>
   { self.submat.clone() }
 }
 
-pub fn rotmat2<N: Clone + Trigonometric + Neg<N>>(angle: N) -> Rotmat<Mat2<N>>
+impl<N: Clone + Trigonometric + Neg<N>> Rotmat<Mat2<N>>
 {
-  let (sia, coa) = angle.sin_cos();
+  pub fn from_angle(angle: N) -> Rotmat<Mat2<N>>
+  {
+    let (sia, coa) = angle.sin_cos();
 
-  Rotmat
-  { submat: Mat2::new(coa.clone(), -sia, sia.clone(), coa) }
+    Rotmat { submat: Mat2::new(coa.clone(), -sia, sia.clone(), coa) }
+  }
 }
 
-pub fn rotmat3<N: Clone + Trigonometric + DivisionRing + Algebraic>
-(axisangle: Vec3<N>) -> Rotmat<Mat3<N>>
+impl<N: Clone + Trigonometric + DivisionRing + Algebraic> Rotmat<Mat3<N>>
 {
-  if axisangle.sqnorm().is_zero()
-  { One::one() }
-  else
+  pub fn from_axis_angle(axisangle: Vec3<N>) -> Rotmat<Mat3<N>>
   {
-    let mut axis   = axisangle;
-    let angle      = axis.normalize();
-    let _1         = One::one::<N>();
-    let ux         = axis.x.clone();
-    let uy         = axis.y.clone();
-    let uz         = axis.z.clone();
-    let sqx        = ux * ux;
-    let sqy        = uy * uy;
-    let sqz        = uz * uz;
-    let (sin, cos) = angle.sin_cos();
-    let one_m_cos  = _1 - cos;
+    if axisangle.sqnorm().is_zero()
+    { One::one() }
+    else
+    {
+      let mut axis   = axisangle;
+      let angle      = axis.normalize();
+      let _1         = One::one::<N>();
+      let ux         = axis.x.clone();
+      let uy         = axis.y.clone();
+      let uz         = axis.z.clone();
+      let sqx        = ux * ux;
+      let sqy        = uy * uy;
+      let sqz        = uz * uz;
+      let (sin, cos) = angle.sin_cos();
+      let one_m_cos  = _1 - cos;
 
-    Rotmat {
-      submat: Mat3::new(
-        (sqx + (_1 - sqx) * cos),
-        (ux * uy * one_m_cos - uz * sin),
-        (ux * uz * one_m_cos + uy * sin),
+      Rotmat {
+        submat: Mat3::new(
+                  (sqx + (_1 - sqx) * cos),
+                  (ux * uy * one_m_cos - uz * sin),
+                  (ux * uz * one_m_cos + uy * sin),
 
-        (ux * uy * one_m_cos + uz * sin),
-        (sqy + (_1 - sqy) * cos),
-        (uy * uz * one_m_cos - ux * sin),
+                  (ux * uy * one_m_cos + uz * sin),
+                  (sqy + (_1 - sqy) * cos),
+                  (uy * uz * one_m_cos - ux * sin),
 
-        (ux * uz * one_m_cos - uy * sin),
-        (uy * uz * one_m_cos + ux * sin),
-        (sqz + (_1 - sqz) * cos))
+                  (ux * uz * one_m_cos - uy * sin),
+                  (uy * uz * one_m_cos + ux * sin),
+                  (sqz + (_1 - sqz) * cos))
+      }
     }
   }
 }
 
+impl<N: Clone + DivisionRing + Algebraic> Rotmat<Mat3<N>>
+{
+  pub fn look_at(&mut self, at: &Vec3<N>, up: &Vec3<N>)
+  {
+    let zaxis = at.normalized();
+    let xaxis = up.cross(&zaxis).normalized();
+    let yaxis = zaxis.cross(&xaxis);
+
+    self.submat = Mat3::new(xaxis.x.clone(), yaxis.x.clone(), zaxis.x.clone(),
+                            xaxis.y.clone(), yaxis.y.clone(), zaxis.y.clone(),
+                            xaxis.z        , yaxis.z        , zaxis.z)
+  }
+}
+
 impl<N: Trigonometric + DivisionRing + Clone>
 Rotation<Vec1<N>> for Rotmat<Mat2<N>>
 {
@@ -90,7 +109,7 @@ Rotatable<Vec1<N>, Rotmat<Mat2<N>>> for Rotmat<Mat2<N>>
 {
   #[inline]
   fn rotated(&self, rot: &Vec1<N>) -> Rotmat<Mat2<N>>
-  { rotmat2(rot.x.clone()) * *self }
+  { Rotmat::from_angle(rot.x.clone()) * *self }
 }
 
 impl<N: Clone + Trigonometric + DivisionRing + Algebraic>
@@ -115,14 +134,14 @@ Rotatable<Vec3<N>, Rotmat<Mat3<N>>> for Rotmat<Mat3<N>>
 {
   #[inline]
   fn rotated(&self, axisangle: &Vec3<N>) -> Rotmat<Mat3<N>>
-  { rotmat3(axisangle.clone()) * *self }
+  { Rotmat::from_axis_angle(axisangle.clone()) * *self }
 }
 
 impl<N: Clone + Rand + Trigonometric + Neg<N>> Rand for Rotmat<Mat2<N>>
 {
   #[inline]
   fn rand<R: Rng>(rng: &mut R) -> Rotmat<Mat2<N>>
-  { rotmat2(rng.gen()) }
+  { Rotmat::from_angle(rng.gen()) }
 }
 
 impl<M: RMul<V> + LMul<V>, V> Rotate<V> for Rotmat<M>
@@ -152,7 +171,7 @@ Rand for Rotmat<Mat3<N>>
 {
   #[inline]
   fn rand<R: Rng>(rng: &mut R) -> Rotmat<Mat3<N>>
-  { rotmat3(rng.gen()) }
+  { Rotmat::from_axis_angle(rng.gen()) }
 }
 
 impl<M: Dim> Dim for Rotmat<M>
diff --git a/src/adaptors/transform.rs b/src/adaptors/transform.rs
index 98654c66..48d58aa5 100644
--- a/src/adaptors/transform.rs
+++ b/src/adaptors/transform.rs
@@ -3,6 +3,7 @@ use std::rand::{Rand, Rng, RngUtil};
 use std::cmp::ApproxEq;
 use traits::dim::Dim;
 use traits::inv::Inv;
+use traits::division_ring::DivisionRing;
 use traits::rotation::{Rotation, Rotate, Rotatable};
 use traits::translation::{Translation, Translate, Translatable};
 use Ts = traits::transformation::Transform;
@@ -10,6 +11,9 @@ use traits::transformation::{Transformation, Transformable};
 use traits::rlmul::{RMul, LMul};
 use traits::homogeneous::{ToHomogeneous, FromHomogeneous};
 use traits::column::Column;
+use adaptors::rotmat::Rotmat;
+use vec::Vec3;
+use mat::Mat3;
 
 #[deriving(Eq, ToStr, Clone)]
 pub struct Transform<M, V>
@@ -36,7 +40,16 @@ impl<M: Clone, V: Clone> Transform<M, V>
   { self.subtrans.clone() }
 }
 
-impl<M:Dim, V> Dim for Transform<M, V>
+impl<N: Clone + DivisionRing + Algebraic> Transform<Rotmat<Mat3<N>>, Vec3<N>>
+{
+  pub fn look_at(&mut self, eye: &Vec3<N>, at: &Vec3<N>, up: &Vec3<N>)
+  {
+    self.submat.look_at(&(*at - *eye), up);
+    self.subtrans = self.submat.rotate(&-eye);
+  }
+}
+
+impl<M: Dim, V> Dim for Transform<M, V>
 {
   #[inline]
   fn dim() -> uint