Fix rotation

demo3/src/main.rs

@@ -12,7 +12,7 @@ use clap::{App, Arg};
 use euclid::Size2D;
 use gl::types::{GLchar, GLfloat, GLint, GLsizei, GLsizeiptr, GLuint, GLvoid};
 use jemallocator;
-use pathfinder_geometry::point::Point2DF32;
+use pathfinder_geometry::point::{Point2DF32, Point4DF32};
 use pathfinder_geometry::transform::Transform2DF32;
 use pathfinder_geometry::transform3d::{Perspective, Transform3DF32};
 use pathfinder_renderer::builder::SceneBuilder;
@@ -56,6 +56,7 @@ const FILL_COLORS_TEXTURE_WIDTH: u32 = 256;
 const FILL_COLORS_TEXTURE_HEIGHT: u32 = 256;
 
 const MOUSELOOK_ROTATION_SPEED: f32 = 0.01;
+const CAMERA_VELOCITY: f32 = 0.03;
 
 fn main() {
     let options = Options::get();
@@ -90,30 +91,27 @@ fn main() {
     let (drawable_width, drawable_height) = window.drawable_size();
     let mut renderer = Renderer::new(&Size2D::new(drawable_width, drawable_height));
 
-    //let mut scale = 1.0;
-    //let mut theta = 0.0;
-    let mut camera_position = [0.0, 0.0, 3.0];
-    let (mut camera_roll, mut camera_pitch, mut camera_yaw) = (0.0, 0.0, 0.0);
+    let mut camera_position = Point4DF32::new(0.0, 0.0, 3.0, 1.0);
+    let mut camera_velocity = Point4DF32::new(0.0, 0.0, 0.0, 1.0);
+    let (mut camera_yaw, mut camera_pitch, mut camera_roll) = (0.0, 0.0, 0.0);
 
     let window_size = Size2D::new(MAIN_FRAMEBUFFER_WIDTH, MAIN_FRAMEBUFFER_HEIGHT);
 
     while !exit {
+        let rotation = Transform3DF32::from_rotation(-camera_yaw, -camera_pitch, -camera_roll);
+        camera_position = camera_position + rotation.transform_point(camera_velocity);
+
         let mut transform = Transform3DF32::from_perspective(FRAC_PI_4, 4.0 / 3.0, 0.01, 100.0);
-        transform = transform.post_mul(&Transform3DF32::from_rotation(camera_roll,
+        transform = transform.post_mul(&Transform3DF32::from_rotation(camera_yaw,
                                                                       camera_pitch,
-                                                                      camera_yaw));
-        transform = transform.post_mul(&Transform3DF32::from_translation(-camera_position[0],
-                                                                         -camera_position[1],
-                                                                         -camera_position[2]));
+                                                                      camera_roll));
+        transform = transform.post_mul(&Transform3DF32::from_translation(-camera_position.x(),
+                                                                         -camera_position.y(),
+                                                                         -camera_position.z()));
         let perspective = Perspective::new(&transform, &window_size);
 
         let mut scene = base_scene.clone();
         scene.apply_perspective(&perspective);
-        //scene.transform(&Transform2DF32::from_rotation(theta));
-        //scene.transform(&Transform2DF32::from_scale(&Point2DF32::splat(scale)));
-        //theta += 0.001;
-        //scale -= 0.0003;
-        //scale += 0.0001;
 
         let built_scene = build_scene(&scene, &options);
 
@@ -132,11 +130,18 @@ fn main() {
                 }
                 Event::MouseMotion { xrel, yrel, .. } => {
                     camera_yaw += xrel as f32 * MOUSELOOK_ROTATION_SPEED;
-                    camera_pitch += yrel as f32 * MOUSELOOK_ROTATION_SPEED;
+                    camera_pitch -= yrel as f32 * MOUSELOOK_ROTATION_SPEED;
+                }
+                Event::KeyDown { keycode: Some(Keycode::W), .. } => {
+                    camera_velocity.set_z(-CAMERA_VELOCITY)
+                }
+                Event::KeyDown { keycode: Some(Keycode::S), .. } => {
+                    camera_velocity.set_z(CAMERA_VELOCITY)
+                }
+                Event::KeyUp { keycode: Some(Keycode::W), .. } |
+                Event::KeyUp { keycode: Some(Keycode::S), .. } => {
+                    camera_velocity.set_z(0.0);
                 }
-                /*Event::KeyDown { keycode: Some(Keycode::W), .. } => {
-
-                }*/
                 _ => {}
             }
         }

geometry/src/point.rs

@@ -12,7 +12,7 @@
 
 use crate::simd::F32x4;
 use euclid::Point2D;
-use std::ops::{Add, Mul, Sub};
+use std::ops::{Add, AddAssign, Mul, Sub};
 
 // 2D points.
 
@@ -140,12 +140,32 @@ impl Point4DF32 {
         self.0[3]
     }
 
+    #[inline]
+    pub fn set_z(&mut self, z: f32) {
+        self.0[2] = z
+    }
+
     #[inline]
     pub fn perspective_divide(self) -> Point4DF32 {
         self * Point4DF32::splat(1.0 / self.w())
     }
 }
 
+impl Add<Point4DF32> for Point4DF32 {
+    type Output = Point4DF32;
+    #[inline]
+    fn add(self, other: Point4DF32) -> Point4DF32 {
+        Point4DF32(self.0 + other.0)
+    }
+}
+
+impl AddAssign for Point4DF32 {
+    #[inline]
+    fn add_assign(&mut self, other: Point4DF32) {
+        self.0 += other.0
+    }
+}
+
 impl Mul<Point4DF32> for Point4DF32 {
     type Output = Point4DF32;
     #[inline]

geometry/src/simd.rs

@@ -336,7 +336,7 @@ mod x86 {
     use std::cmp::PartialEq;
     use std::fmt::{self, Debug, Formatter};
     use std::mem;
-    use std::ops::{Add, Index, IndexMut, Mul, Sub};
+    use std::ops::{Add, AddAssign, Index, IndexMut, Mul, Sub};
 
     // 32-bit floats
 
@@ -500,6 +500,13 @@ mod x86 {
         }
     }
 
+    impl AddAssign for F32x4 {
+        #[inline]
+        fn add_assign(&mut self, other: F32x4) {
+            unsafe { self.0 = x86_64::_mm_add_ps(self.0, other.0) }
+        }
+    }
+
     impl Mul<F32x4> for F32x4 {
         type Output = F32x4;
         #[inline]

geometry/src/transform3d.rs

@@ -70,19 +70,19 @@ impl Transform3DF32 {
     }
 
     // TODO(pcwalton): Optimize.
-    pub fn from_rotation(roll: f32, pitch: f32, yaw: f32) -> Transform3DF32 {
-        let (cos_roll, sin_roll) = (roll.cos(), roll.sin());
-        let (cos_pitch, sin_pitch) = (pitch.cos(), pitch.sin());
-        let (cos_yaw, sin_yaw) = (yaw.cos(), yaw.sin());
-        let m00 = cos_yaw * cos_pitch;
-        let m01 = cos_yaw * sin_pitch * sin_roll - sin_yaw * cos_roll;
-        let m02 = cos_yaw * sin_pitch * sin_roll + sin_yaw * sin_roll;
-        let m10 = sin_yaw * cos_pitch;
-        let m11 = sin_yaw * sin_pitch * sin_roll + cos_yaw * cos_roll;
-        let m12 = sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll;
-        let m20 = -sin_pitch;
-        let m21 = cos_pitch * sin_roll;
-        let m22 = cos_pitch * cos_roll;
+    pub fn from_rotation(yaw: f32, pitch: f32, roll: f32) -> Transform3DF32 {
+        let (cos_b, sin_b) = (yaw.cos(),   yaw.sin());
+        let (cos_c, sin_c) = (pitch.cos(), pitch.sin());
+        let (cos_a, sin_a) = (roll.cos(),  roll.sin());
+        let m00 = cos_a * cos_b;
+        let m01 = cos_a * sin_b * sin_c - sin_a * cos_c;
+        let m02 = cos_a * sin_b * cos_c + sin_a * sin_c;
+        let m10 = sin_a * cos_b;
+        let m11 = sin_a * sin_b * sin_c + cos_a * cos_c;
+        let m12 = sin_a * sin_b * cos_c - cos_a * sin_c;
+        let m20 = -sin_b;
+        let m21 = cos_b * sin_c;
+        let m22 = cos_b * cos_c;
         Transform3DF32::row_major(m00, m01, m02, 0.0,
                                   m10, m11, m12, 0.0,
                                   m20, m21, m22, 0.0,