initial commit

engine/libraries/mathlib/CMakeLists.txt

@@ -0,0 +1,10 @@
+PROJECT (MATHLIB CXX)
+
+LIST ( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/ )
+
+SET ( SRCS
+	mathlib.cc
+	main.cc
+	)
+
+ADD_LIBRARY ( mathlib mathlib.cc mathlib.h)

engine/libraries/mathlib/mathlib.cc

@@ -0,0 +1,224 @@
+#include <iostream>
+#include <sstream>
+#include <math.h>
+#include <assert.h>
+
+#include "mathlib.h"
+
+using namespace std;
+
+#ifdef WIN32
+
+void sincos (float rad, double *s, double *c){
+	*s = sin (rad);
+	*c = cos (rad);
+}
+
+#endif
+
+vector3d::vector3d ()
+{
+	values[0] = 0.;
+	values[1] = 0.;
+	values[2] = 0.;
+}
+
+vector3d::vector3d (float u, float v, float w)
+{
+	values[0] = u;
+	values[1] = v;
+	values[2] = w;
+}
+
+vector3d::vector3d (const vector3d &vec)
+{
+	values[0] = vec.values[0];
+	values[1] = vec.values[1];
+	values[2] = vec.values[2];
+}
+
+vector3d vector3d::operator=(const vector3d &vec) {
+	if (this != &vec) {
+		values[0] = vec.values[0];
+		values[1] = vec.values[1];
+		values[2] = vec.values[2];
+	}
+	return *this;
+}
+
+vector3d::~vector3d ()
+{
+}
+
+#ifndef MATHLIB_INLINE
+vector3d vector3d::operator* (const float &f)
+{
+  return vector3d (values[0] * f, values[1] * f, values[2] * f);
+}
+
+vector3d vector3d::operator/ (const float &f)
+{
+  return vector3d (values[0] / f, values[1] / f, values[2] / f);
+}
+
+void vector3d::operator*= (const float &f)
+{
+	values[0] *= f;
+	values[1] *= f;
+	values[2] *= f;
+}
+
+void vector3d::operator/= (const float &f)
+{
+	values[0] /= f;
+	values[1] /= f;
+	values[2] /= f;
+}
+
+float vector3d::operator* (const vector3d &vec)
+{
+  return values[0] * vec.values[0] + values[1] * vec.values[1] + values[2] * vec.values[2];
+}
+
+vector3d vector3d::operator+ (const vector3d &vec)
+{
+  return vector3d (values[0] + vec.values[0], values[1] + vec.values[1], values[2] + vec.values[2]);
+}
+
+vector3d vector3d::operator- (const vector3d &vec)
+{
+  return vector3d (values[0] - vec.values[0], values[1] - vec.values[1], values[2] - vec.values[2]);
+}
+
+void vector3d::operator+= (const vector3d &vec)
+{
+  values[0] += vec.values[0];
+  values[1] += vec.values[1];
+  values[2] += vec.values[2];
+}
+
+void vector3d::operator-= (const vector3d &vec)
+{
+  values[0] -= vec.values[0];
+  values[1] -= vec.values[1];
+  values[2] -= vec.values[2];
+}
+
+bool vector3d::operator== (const vector3d &vec)
+{
+  if ( (values[0]==vec.values[0]) && (values[1]==vec.values[1]) && (values[2]==vec.values[2]) )
+    return true;
+  return false;
+}
+
+bool vector3d::operator!= (const vector3d &vec)
+{
+	return ! (operator==(vec));
+}
+
+vector3d vector3d::cross (const vector3d &vec)
+{
+  return vector3d (values[1] * vec.values[2] - values[2] * vec.values[1],
+      values[2] * vec.values[0] - values[0] * vec.values[2],
+      values[0] * vec.values[1] - values[1] * vec.values[0]);
+}
+
+float vector3d::length2 ()
+{
+  return values[0]*values[0] + values[1]*values[1] + values[2]*values[2];
+}
+
+float vector3d::length ()
+{
+  return sqrt (values[0]*values[0] + values[1]*values[1] + values[2]*values[2]);
+}
+#endif
+
+vector3d& vector3d::rotate_x (float angle)
+{
+ 	vector3d old (*this);
+	double s,c;
+	sincos (angle, &s, &c);
+
+	values[0] = old[0];
+	values[1] = c * old[1] - s * old[2];
+	values[2] = s * old[1] + c * old[2];
+
+ 	return *this;
+}
+
+vector3d& vector3d::rotate_y (float angle)
+{
+	vector3d old (*this);
+	double s,c;
+	sincos (angle, &s, &c);
+
+	values[0] = c * old[0] + s * old[2];
+	values[1] = old[1];
+	values[2] =	- s * old[0] + c * old[2];
+
+	return *this;
+}
+
+vector3d& vector3d::rotate_z (float angle)
+{
+ 	vector3d old (*this);
+	double s,c;
+	sincos (angle, &s, &c);
+
+	values[0] = c * old[0] + -s * old[1];
+	values[1] = s * old[0] + c * old[1];
+	values[2] = old[2];
+
+	return *this;
+}
+
+vector3d& vector3d::normalize () {
+	float norm = length ();
+	values [0] /= norm;
+	values [1] /= norm;
+	values [2] /= norm;
+
+	return *this;
+}
+
+void vector3d::print ()
+{
+  std::cout << values[0] << " " << values[1] << " " << values[2] << std::endl;
+}
+
+void vector3d::print (const char *str)
+{
+  std::cout << str << values[0] << " " << values[1] << " " << values[2] << std::endl;
+}
+
+vector3d cross_product (vector3d &a, vector3d &b) {
+	return a.cross (b);
+}
+
+inline bool point_within (vector3d *a, vector3d *b, vector3d *point);
+
+vector3d integrate_rk45 (float t, vector3d& ydot, vector3d& y)
+{
+  vector3d k1 = ydot * t;
+  vector3d k2 = (ydot + k1 / 2) * t;
+  vector3d k3 = (ydot + k2 / 2 ) * t;
+  vector3d k4 = (ydot + k3) * t;
+
+  return y + (k1 + k2 * 2 + k3 * 2 + k4) / 6;
+}
+
+vector3d integrate (float t, vector3d& ydot, vector3d& y)
+{
+  return integrate_rk45 (t, ydot, y);
+}
+
+int solve_quadratic (float a, float b, float c, float *x1, float *x2) {
+	if (b*b - 4 * a * c < 0)
+		return 0;
+	*x1 = static_cast<float> ((-b + sqrt (b*b - 4 * a * c)) / (2 * a));
+	*x2 = static_cast<float> ((-b - sqrt (b*b - 4 * a * c)) / (2 * a));
+
+	return 1;
+}
+

engine/libraries/mathlib/mathlib.h

@@ -0,0 +1,179 @@
+#ifndef __MATHLIB_H
+#define __MATHLIB_H
+
+#include <iostream>
+#include <cmath>
+#include <assert.h>
+
+#ifdef WIN32
+
+#define M_PI 3.14159265
+void sincos (float rad, double *s, double *c);
+
+#endif
+
+#define MATHLIB_INLINE
+
+struct vector3d
+{
+	float values[3];
+
+	vector3d ();
+	vector3d (float u, float v, float w);
+	vector3d (const vector3d &vec);
+	vector3d operator= (const vector3d &vec);
+	~vector3d ();
+
+	// Setter and getter
+	inline void setValues (float u, float v, float w) {
+		values[0] = u;
+		values[1] = v;
+		values[2] = w;
+	}
+
+	inline float& operator[] (const int index) {
+		assert (index >= 0);
+		assert (index <= 3);
+
+		return values[index];
+	}
+
+	vector3d operator* (const float &f);
+	vector3d operator/ (const float &f);
+	void operator*= (const float &f);
+	void operator/= (const float &f);
+
+	float operator* (const vector3d &vec);
+	vector3d operator+ (const vector3d &vec);
+	vector3d operator- (const vector3d &vec);
+	void operator+= (const vector3d &vec);
+	void operator-= (const vector3d &vec);
+	bool operator== (const vector3d &vec);
+	bool operator!= (const vector3d &vec);
+	vector3d cross (const vector3d &vec);
+
+	float length2 ();
+	float length ();
+	float get_angle () {
+		return atan2 (values[1], values[0]);
+	}
+
+	vector3d &rotate_x (float angle);
+	vector3d &rotate_y (float angle);
+	vector3d &rotate_z (float angle);
+	vector3d &normalize ();
+
+	void print ();
+	void print (const char* str);
+};
+
+#ifdef MATHLIB_INLINE
+
+inline
+vector3d vector3d::operator* (const float &f)
+{
+  return vector3d (values[0] * f, values[1] * f, values[2] * f);
+}
+
+inline
+vector3d vector3d::operator/ (const float &f)
+{
+  return vector3d (values[0] / f, values[1] / f, values[2] / f);
+}
+
+inline
+void vector3d::operator*= (const float &f)
+{
+	values[0] *= f;
+	values[1] *= f;
+	values[2] *= f;
+}
+
+inline
+void vector3d::operator/= (const float &f)
+{
+	values[0] /= f;
+	values[1] /= f;
+	values[2] /= f;
+}
+
+inline
+float vector3d::operator* (const vector3d &vec)
+{
+  return values[0] * vec.values[0] + values[1] * vec.values[1] + values[2] * vec.values[2];
+}
+
+inline
+vector3d vector3d::operator+ (const vector3d &vec)
+{
+  return vector3d (values[0] + vec.values[0], values[1] + vec.values[1], values[2] + vec.values[2]);
+}
+
+inline
+vector3d vector3d::operator- (const vector3d &vec)
+{
+  return vector3d (values[0] - vec.values[0], values[1] - vec.values[1], values[2] - vec.values[2]);
+}
+
+inline
+void vector3d::operator+= (const vector3d &vec)
+{
+  values[0] += vec.values[0];
+  values[1] += vec.values[1];
+  values[2] += vec.values[2];
+}
+
+inline
+void vector3d::operator-= (const vector3d &vec)
+{
+  values[0] -= vec.values[0];
+  values[1] -= vec.values[1];
+  values[2] -= vec.values[2];
+}
+
+inline
+bool vector3d::operator== (const vector3d &vec)
+{
+  if ( (values[0]==vec.values[0]) && (values[1]==vec.values[1]) && (values[2]==vec.values[2]) )
+    return true;
+  return false;
+}
+
+inline
+bool vector3d::operator!= (const vector3d &vec)
+{
+	return ! (operator==(vec));
+}
+
+inline
+vector3d vector3d::cross (const vector3d &vec)
+{
+  return vector3d (values[1] * vec.values[2] - values[2] * vec.values[1],
+      values[2] * vec.values[0] - values[0] * vec.values[2],
+      values[0] * vec.values[1] - values[1] * vec.values[0]);
+}
+
+inline
+float vector3d::length2 ()
+{
+  return values[0]*values[0] + values[1]*values[1] + values[2]*values[2];
+}
+
+inline
+float vector3d::length ()
+{
+  return sqrt (values[0]*values[0] + values[1]*values[1] + values[2]*values[2]);
+}
+#endif
+
+vector3d cross_product (vector3d &a, vector3d &b);
+
+inline bool point_within (vector3d *a, vector3d *b, vector3d *point);
+
+vector3d integrate_rk45 (float t,vector3d& ydot, vector3d& y);
+
+vector3d integrate (float t, vector3d& ydot, vector3d& y);
+
+int solve_quadratic (float a, float b, float c, float *x1, float *x2);
+
+#endif /* __MATHLIB_H */