From 6612d1e0a9fa8f4085d0a951ff14c0dcd58e3431 Mon Sep 17 00:00:00 2001
From: blobt <klm777x7@gmai.com>
Date: Wed, 17 Feb 2021 13:10:50 +0800
Subject: [PATCH] =?UTF-8?q?=E5=9B=9B=E5=85=83=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .vs/3dMath/v15/.suo | Bin 39936 -> 39424 bytes
 GMath.cpp           | 117 ++++++++++++++++++++++++++++++++++++++++++++
 GMath.h             |  21 +++++++-
 main.cpp            |  13 ++---
 4 files changed, 141 insertions(+), 10 deletions(-)

diff --git a/.vs/3dMath/v15/.suo b/.vs/3dMath/v15/.suo
index ed7b39461a9173fb34da11887f9e48f94bb6ecd8..89e839a31fa9246176dec8954fd08919367c3d76 100644
GIT binary patch
delta 2173
zcmdT_Z){Ul6o2>jb^Wu~b^o_+jBV^+S-a7<KVTGyrh|<Y*Uk{Ou&~j>h`OOOyhSl{
zYXX{}l|YY)5`upgK8Rloy~OYZS%@(UCd#4)6Z~R=n#czusU{^dJ?HJU>oQ0ne)1;2
zckVm)ymRk4zjN-3UgFV<{N!e4zp%y0Sq{|lYTI0N?1SIr8>~W1@*q?4BuQ#P{DT7)
zHq=AX<en))MGm6QSWr-sAsLmSEJipHHba+-vcQ;=O};U2Ls^MfgIH_ms!`S(bF!te
zh-g4vBce%?;EY`BQVkmy%63Etq7$)rq2I&ed?zc^!-)poXuOT#Y=lYP2Y!<ZNq${%
zpyA4>EFBIRmHKx0sDqc}qHQHvno^wmjQ4dYd$QK(tMaUOa$kt3K`fr?Z)a8Lt_*&#
zZgy=)RX2i;y#vvU2q4hx+l1ZE9gN`{C6Sy`#)Xz2V~IRJ2fy+<%;!%jbWl^41!#`g
zYz2=O`W?)J`gCe^zO=$!Fz@j~66Hdq4t(4OT2au!)}y60%Zo_5LR_grO`58u@3(#f
z{;JCmEv^wY#pjsPiWWp#`g~?Oz6z;=-bdX%onG3<e4JcDs_H9+9M@)T0LisMhh#^M
zB}*%`>L8}Xq{chtQ$B0l`-M!jWMWKuF~(-pq>FSZnwF4Kfy(Ty;Yn3y<&YW+!aVna
z*1XG_E=K*EWOLB0Q=jpzd4jaF?LG7~gplA~W%sJo32jY`#T&a6Drqd+OvXTKye?Bu
zEIVupDbz&NIGMas)k}~n2`JQW>7LP(bpjO}j+9>0D_ScH=}Ds&$$Sm?;u5K%0J~4U
zl!P8gW7Rc>L>XFf;QKNFT6HYp?qN$=TOW%z%0jNW!j?$^=Bv+D+r7T2?cwpmqapYy
zTnQhCy%p+X8@*}=Y0bD5Rs8h%bJn+q3l!)d^OaL`r0<XJr^DK_t@|e?X5~DL-94)^
z=Yi3K;mL7rL_0Lm=~p|2^Myam4_yGUSxt_Q_dFukIZIrNyA8c;1$X8M9`+a<X$t&D
z2B{ZUGYF}cpq1_(gIG7b(z;V;d&9D_UL;!;mgp(AILHePlDSYD+`@9jNbnO293Pkl
z+dx!U2LIxAY9x;6nt``=_raARn;0I-;ib3eYSG8Kx!X9W3%cty!zM?3y|;wBbh>mK
z_naN6ql=$v*ChUKl6hiZlsP!6i9e3lR3DOF4bS)*EN$&RuiDlI9-liN*_6NU=x3~a
z)7ckOHx~}^yC-|#!vm$TH!_$nMVvR@ZF=H-@8jRc+HXyNX)Kh%oyb#Ndp^DM^S0j0
zWuFhcKXd8g)JJp>N)3%^4C#6NFQqW}-?6Y#c(j`s>eA}+Q@p%?hs+O;!qwA#!a4jM
z^ZbZjYRHw;I`<1cUq$FKp<G>T=7I;Lxz>_<4F7r5t|lpyS{NR3!<)Njkb)Rnp%w28
kKF66=EbckTOMlJE{hv4|1|L56_ePHXZ}Kj#p5Ds;1;5(n_5c6?

delta 2149
zcmds2TTEO<7(O$*3kP}*EOK2aAh1wwg<UvwNmI1VF1T3C?k27Al3FP477V4N9<Yr>
zSEG`)V7mK1)E5)e_)=qxWlZ#~74W4b(nceVCMG@@4KF2T(_|A{{Qt98uubf%lYE((
zZ|2PWm+zkw{)&gc;g=4u>T7+4oSC4G*XB)~sg7+)>&zymxsNGkNs{&=wy;4&vqQ|(
zeDnz<$`A#JlH_6z_IZdRM1GPh#a>O0W$X(P5AYyIlB>kN3b7ke58uk=as!U+2)JZ!
zSIcqWM06nrXgM<6%N|4me?<8Z+nccV?3eFKY@O|cY5qMdb0_E~AGl3Uh~;XLVKdY4
zyW+YYJb^r*)}1w``5?|~X;%^XT7f7=SP?aeI~~fCI3c@tVGnoY1GZ;yK;L??Co7*t
z97YVmZSxO{`ECww>-#dDL%R}jQ+%p1qoAc&p{r|LWeh2bX@*wjS+vA$IHH#vk@sXR
zLJSF8Hldd$kEVF1&aj-PiHO@)8LV)Jh0Jq8yydXcfg^G+z4JqyFgVwGGZDZxzd})O
z%-WI%IHAx;aKCsUX_ns*#}3o{bQwI<A`vU~bA@gtjihcDdK3Cuv!aj~NF=ERh}HO1
zMj_A5VufDo!-&-&)tN;{UFL{Gj<$Lf3$l;toyG7D;+?AsrHmArG2+B6!(@_ZhICqm
z*>jMYN&Ij-)Bp~i2YQQNsYW8Dek1LA?QUT`vB{$LWH#+bWGbD^S$6cY1sRfln$;V9
zuw3G_e1zo75U*QRsAiPxEwTE9(DlCgsMjmkXV#kGtanIhL!vc9+C|K(Fz{=QIbB^n
z?cGke6KsZn)>xsnJ7xz)CSIq=QG4s^r}DxpMG6E49p#jCDqG6Tsgd!?j?t+pRoa8T
zwOr|8(aL3&k<pZ<wDGaZHB};&Wk<5g5FnD&UJZsurYH2{`l%_+)!!`&_gNT(UG3uF
z=^HtVWp1uY<WIIvK(AO!+-1U*^j*3Q1J&fTP<cuauU>tGjwmTRbhO4)){HdeEOTBR
zrsLu2TJ(6BnNqMV<Md_f4Wx|kGAzMew9)zWNfDa)LRRS&q(*%{)CKyOB8~?>%Z0fY
z!*G6XUX;G{CugeIx)9<;XXqg^+UE6laIA`%g-LGvImJpHH-5*lA3^4r#NVUuF@;_c
z$`qGO5_D)@)zRx}@9s5H8+;BrwJOut?3=I^?6q3&y}e#@&edxfzVe-RKe`N`jFrK$
z&<oapJU;RD<%Xuuk6z`Y6UUxUE|kMxp+lbi*EWCriCtgbfA{9S+20O@@h&576oJ!R
zl3v4Kzy|8%E(&P_U%6i}-n2%I_DblAc;n=t*%~ZZTKafnq%EzqTjP#s{fhB)Y?F1O
zW*pr7XhlcuUYTlxZb?+!V^vs{QDKI8+hp0_uen6wq^0P;r7n*OfF2JIT=*a?<}Uuq
l{+&jcS-5C*@9<q%$2J*sX%)gYvC95GN#?-<v2^(<{tuc$;fMeL

diff --git a/GMath.cpp b/GMath.cpp
index dc2810f..e16020e 100644
--- a/GMath.cpp
+++ b/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;
+}
diff --git a/GMath.h b/GMath.h
index 102ed3c..2b0340f 100644
--- a/GMath.h
+++ b/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);
 };
\ No newline at end of file
diff --git a/main.cpp b/main.cpp
index 7fe3e78..d040f47 100644
--- a/main.cpp
+++ b/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;
 }
\ No newline at end of file