4 files changed, 60 insertions, 4 deletions
diff --git a/core/math/camera_matrix.cpp b/core/math/camera_matrix.cpp
index 52d77b6ebc..a60dea7379 100644
--- a/core/math/camera_matrix.cpp
+++ b/core/math/camera_matrix.cpp
@@ -67,9 +67,10 @@ Plane CameraMatrix::xform4(const Plane& p_vec4) {
 
 void CameraMatrix::set_perspective(float p_fovy_degrees, float p_aspect, float p_z_near, float p_z_far,bool p_flip_fov) {
 
-	if (p_flip_fov)
-		p_fovy_degrees=get_fovy(p_fovy_degrees,p_aspect);
+	if (p_flip_fov) {
+		p_fovy_degrees=get_fovy(p_fovy_degrees,1.0/p_aspect);
 
+	}
 
 	float sine, cotangent, deltaZ;
 	float radians = p_fovy_degrees / 2.0 * Math_PI / 180.0;
@@ -110,7 +111,7 @@ void CameraMatrix::set_orthogonal(float p_left, float p_right, float p_bottom, f
 
 void CameraMatrix::set_orthogonal(float p_size, float p_aspect, float p_znear, float p_zfar,bool p_flip_fov) {
 
-	if (p_flip_fov) {
+	if (!p_flip_fov) {
 		p_size*=p_aspect;
 	}
 
diff --git a/core/math/camera_matrix.h b/core/math/camera_matrix.h
index 6ffcb0ed0b..767236ea04 100644
--- a/core/math/camera_matrix.h
+++ b/core/math/camera_matrix.h
@@ -60,7 +60,7 @@ struct CameraMatrix {
 
 	static float get_fovy(float p_fovx,float p_aspect) {
 
-		return Math::atan(p_aspect * Math::tan(p_fovx * 0.5))*2.0;
+		return Math::rad2deg(Math::atan(p_aspect * Math::tan(Math::deg2rad(p_fovx) * 0.5))*2.0);
 	}
 
 	float get_z_far() const;
diff --git a/core/math/quat.h b/core/math/quat.h
index d326073033..04901116b8 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -64,6 +64,22 @@ public:
 	Quat operator*(const Quat& q) const;
 
 
+
+	Quat operator*(const Vector3& v) const
+	{
+		return Quat( w * v.x + y * v.z - z * v.y,
+			w * v.y + z * v.x - x * v.z,
+			w * v.z + x * v.y - y * v.x,
+			-x * v.x - y * v.y - z * v.z);
+	}
+
+	_FORCE_INLINE_ Vector3 xform(const Vector3& v) {
+
+		Quat q = *this * v;
+		q *= this->inverse();
+		return Vector3(q.x,q.y,q.z);
+	}
+
 	_FORCE_INLINE_ void operator+=(const Quat& q);
 	_FORCE_INLINE_ void operator-=(const Quat& q);
 	_FORCE_INLINE_ void operator*=(const real_t& s);
@@ -87,6 +103,29 @@ public:
 		x=p_x; y=p_y; z=p_z; w=p_w;	
 	}
 	Quat(const Vector3& axis, const real_t& angle);
+
+	Quat(const Vector3& v0, const Vector3& v1) // shortest arc
+	{
+		Vector3 c = v0.cross(v1);
+		real_t  d = v0.dot(v1);
+
+		if (d < -1.0 + CMP_EPSILON) {
+			x=0;
+			y=1;
+			z=0;
+			w=0;
+		} else {
+
+			real_t  s = Math::sqrt((1.0f + d) * 2.0f);
+			real_t rs = 1.0f / s;
+
+			x=c.x*rs;
+			y=c.y*rs;
+			z=c.z*rs;
+			w=s * 0.5;
+		}
+	}
+
 	inline Quat() {x=y=z=0; w=1; }
 	
 
diff --git a/core/math/vector3.h b/core/math/vector3.h
index 959f7cd0a8..d2f2408829 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -111,6 +111,12 @@ struct Vector3 {
         _FORCE_INLINE_ real_t distance_to(const Vector3& p_b) const;
         _FORCE_INLINE_ real_t distance_squared_to(const Vector3& p_b) const;
 
+
+
+	_FORCE_INLINE_ Vector3 slide(const Vector3& p_vec) const;
+	_FORCE_INLINE_ Vector3 reflect(const Vector3& p_vec) const;
+
+
         /* Operators */
 
         _FORCE_INLINE_ Vector3& operator+=(const Vector3& p_v);
@@ -368,6 +374,16 @@ void Vector3::zero() {
 	x=y=z=0;
 }
 
+Vector3 Vector3::slide(const Vector3& p_vec) const {
+
+	return p_vec - *this * this->dot(p_vec);
+}
+Vector3 Vector3::reflect(const Vector3& p_vec) const {
+
+	return p_vec - *this * this->dot(p_vec) * 2.0;
+
+}
+
 #endif
 
 #endif // VECTOR3_H