四元数

GMath.cpp

@@ -1153,3 +1153,120 @@ bool intersect_line_plane(GPoint3 & p, const GLine & l, const GPlane & pi)
 
 	return true;
 }
+
+bool intersect_line_triangle(GPoint3 & p, const GLine & l, const GPoint3 & p1, const GPoint3 & p2, const GPoint3 & p3)
+{
+	GVector3 e1, e2, u, v, w;
+	float a, b, t, det;
+
+	e1 = p2 - p1;
+	e2 = p3 - p1;
+	u = l.v ^ e2;
+	det = e1 * u;
+
+	if (EQ_ZERO(det, PRECISION)) {
+		return false;
+	}
+
+	w = l.p - p1;
+
+	a = w * u / det;
+	if (a < 0.0f || a > 1.0f) {
+		return false;
+	}
+
+	v = w ^ e1;
+	b = l.v * v / det;
+	if (b < 0.0f || b > 1.0f) {
+		return false;
+	}
+
+	t = e2 * v / det;
+	p = l(t);
+
+	return true; 
+}
+
+GQuater::GQuater(float w, float x, float y, float z)
+{
+	this->w = w;
+	this->x = x;
+	this->y = y;
+	this->z = z;
+}
+
+GQuater::GQuater(const GQuater & copy)
+{
+	w = copy.w;
+	x = copy.x;
+	y = copy.y;
+	z = copy.z;
+}
+
+GQuater::GQuater(GVector3 axis, float theta, bool radian)
+{
+	//q=(cosX, sinX(a,b,c))
+
+	float rad, sn, cs;
+	axis.normalize();
+
+	if (!radian) {
+		rad = theta * M_PI / 360; //因为旋转的是2倍X，所以这里除以360
+	}
+
+	sn = sin(rad);
+	cs = cos(rad);
+
+	sn = (abs(sn) < PRECISION) ? 0.0f : sn;
+	cs = (abs(cs) < PRECISION) ? 0.0f : cs;
+
+	w = cs;
+	x = sn * axis[0];
+	y = sn * axis[1];
+	z = sn * axis[2];
+
+}
+
+GQuater & GQuater::operator=(const GQuater & rhs)
+{
+	w = rhs.w;
+	x = rhs.x;
+	y = rhs.y;
+	z = rhs.z;
+
+	return *this;
+}
+
+GQuater & GQuater::operator+=(const GQuater & rhs)
+{
+	w += rhs.w;
+	x += rhs.x;
+	y += rhs.y;
+	z += rhs.z;
+
+	return *this;
+}
+
+GQuater & GQuater::operator-=(const GQuater & rhs)
+{
+	w -= rhs.w;
+	x -= rhs.x;
+	y -= rhs.y;
+	z -= rhs.z;
+
+	return *this;
+}
+
+GQuater & GQuater::operator*=(const GQuater & rhs)
+{
+	float nw, nx, ny, nz = 0;
+	nw = w * rhs.w - x*rhs.x - y * rhs.y - z * rhs.z;
+	nx = w * rhs.x + rhs.w*x + y * rhs.z - z * rhs.y;
+	ny = w * rhs.y + rhs.w*y + z * rhs.x - x * rhs.z;
+	nz = w * rhs.z + rhs.w*z + x * rhs.y - y * rhs.x;
+	w = nw;
+	x = nx;
+	y = ny;
+	z = nz;
+	return *this;
+}

GMath.h

@@ -13,8 +13,9 @@
 #define EQ(X,Y,EPS) (ABS((X)-(Y))<EPS)//通过误差去判断，两个数是否相等
 #define EQ_ZERO(X,EPS) (ABS((X))<EPS)
 #define ARR_ZERO(A, N) memset((A), 0, sizeof(A[0])*(N))
-#define MIN(x,y) (x)<(y)?(x):(y);
+#define MIN(x,y) (x)<(y)?(x):(y)
 #define SWAP(type, x, y) {type tmp=(x); (x)=(y); (y) = tmp;}
+#define M_PI 3.141592
 
 using namespace std;
 
@@ -22,6 +23,7 @@ class GVector3;
 class GVector;
 class GMatrix;
 class GPlane;
+class GQuater;
 typedef GVector3 GPoint3;
 
 class GVector3 {
@@ -228,6 +230,7 @@ public:
 	friend float dist(const GPoint3& q, const GLine& l);
 
 	friend bool intersect_line_plane(GPoint3& p, const GLine& l, const GPlane& pi);
+	friend bool intersect_line_triangle(GPoint3& p, const GLine& l, const GPoint3& p1, const GPoint3& p2, const GPoint3& p3);
 };
 
 class GPlane {
@@ -247,4 +250,20 @@ public:
 	friend float dist(const GPlane& pi, const GPoint3& p);
 
 	friend bool intersect_line_plane(GPoint3& p, const GLine& l, const GPlane& pi);
+
+	friend bool intersect_line_triangle(GPoint3& p, const GLine& l, const GPoint3& p1, const GPoint3& p2, const GPoint3& p3);
+};
+
+class GQuater {
+private:
+	float w, x, y, z;
+public:
+	GQuater(float w = 1.0f, float x = 0.0f, float y = 0.0f, float z = 0.0f);
+	GQuater(const GQuater& copy);
+	GQuater(GVector3 axis, float theta, bool radian = false);
+	GQuater& operator=(const GQuater& rhs);
+
+	GQuater& operator+=(const GQuater& rhs);
+	GQuater& operator-=(const GQuater& rhs);
+	GQuater& operator*=(const GQuater& rhs);
 };

main.cpp

@@ -2,14 +2,9 @@
 
 int main() {
 
-	GPlane PI(1.0f, 1.0f, 1.0f, -1.0);
-	GLine l(GPoint3(0.0f, 0.0f, 0.0f), GVector3(1.0f, 1.0f, 1.0f));
+	GQuater q1(1.0f, 1.0f, 2.0f, 1.0f);
+	GQuater q2(1.0f/7.0f, -1.0f/7.0f, -2.0f/7.0f, -1.0f/7.0f);
 
-	GPoint3 p;
-
-	bool ret = intersect_line_plane(p, l, PI);
-
-	cout << p << endl;
-
-	return 0;
+	q1 *= q2;
+ 	return 0;
 }