diff --git a/.vs/3dMath/v15/.suo b/.vs/3dMath/v15/.suo index ed7b394..89e839a 100644 Binary files a/.vs/3dMath/v15/.suo and b/.vs/3dMath/v15/.suo differ 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))