diff options
Diffstat (limited to 'src/Quaternions.cpp')
| -rw-r--r-- | src/Quaternions.cpp | 747 |
1 files changed, 747 insertions, 0 deletions
diff --git a/src/Quaternions.cpp b/src/Quaternions.cpp new file mode 100644 index 0000000..464c0f2 --- /dev/null +++ b/src/Quaternions.cpp @@ -0,0 +1,747 @@ +#include "Quaternions.h" + +// Functions +quaternion Quat_Mult(quaternion q1, quaternion q2) +{ + quaternion QResult; + float a, b, c, d, e, f, g, h; + a = (q1.w + q1.x) * (q2.w + q2.x); + b = (q1.z - q1.y) * (q2.y - q2.z); + c = (q1.w - q1.x) * (q2.y + q2.z); + d = (q1.y + q1.z) * (q2.w - q2.x); + e = (q1.x + q1.z) * (q2.x + q2.y); + f = (q1.x - q1.z) * (q2.x - q2.y); + g = (q1.w + q1.y) * (q2.w - q2.z); + h = (q1.w - q1.y) * (q2.w + q2.z); + QResult.w = b + (-e - f + g + h) / 2; + QResult.x = a - (e + f + g + h) / 2; + QResult.y = c + (e - f + g - h) / 2; + QResult.z = d + (e - f - g + h) / 2; + return QResult; +} + +XYZ XYZ::operator+(XYZ add){ + XYZ ne; + ne=add; + ne.x+=x; + ne.y+=y; + ne.z+=z; + return ne; +} + +XYZ XYZ::operator-(XYZ add){ + XYZ ne; + ne=add; + ne.x=x-ne.x; + ne.y=y-ne.y; + ne.z=z-ne.z; + return ne; +} + +XYZ XYZ::operator*(float add){ + XYZ ne; + ne.x=x*add; + ne.y=y*add; + ne.z=z*add; + return ne; +} + +XYZ XYZ::operator*(XYZ add){ + XYZ ne; + ne.x=x*add.x; + ne.y=y*add.y; + ne.z=z*add.z; + return ne; +} + +XYZ XYZ::operator/(float add){ + XYZ ne; + ne.x=x/add; + ne.y=y/add; + ne.z=z/add; + return ne; +} + +void XYZ::operator+=(XYZ add){ + x+=add.x; + y+=add.y; + z+=add.z; +} + +void XYZ::operator-=(XYZ add){ + x=x-add.x; + y=y-add.y; + z=z-add.z; +} + +void XYZ::operator*=(float add){ + x=x*add; + y=y*add; + z=z*add; +} + +void XYZ::operator*=(XYZ add){ + x=x*add.x; + y=y*add.y; + z=z*add.z; +} + +void XYZ::operator/=(float add){ + x=x/add; + y=y/add; + z=z/add; +} + +void XYZ::operator=(float add){ + x=add; + y=add; + z=add; +} + +void XYZ::vec(Vector add){ + x=add.x; + y=add.y; + z=add.z; +} + +bool XYZ::operator==(XYZ add){ + if(x==add.x&&y==add.y&&z==add.z)return 1; + return 0; +} + +quaternion To_Quat(Matrix_t m) +{ + // From Jason Shankel, (C) 2000. + quaternion Quat; + + double Tr = m[0][0] + m[1][1] + m[2][2] + 1.0, fourD; + double q[4]; + + int i,j,k; + if (Tr >= 1.0) + { + fourD = 2.0*fast_sqrt(Tr); + q[3] = fourD/4.0; + q[0] = (m[2][1] - m[1][2]) / fourD; + q[1] = (m[0][2] - m[2][0]) / fourD; + q[2] = (m[1][0] - m[0][1]) / fourD; + } + else + { + if (m[0][0] > m[1][1]) + { + i = 0; + } + else + { + i = 1; + } + if (m[2][2] > m[i][i]) + { + i = 2; + } + j = (i+1)%3; + k = (j+1)%3; + fourD = 2.0*fast_sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0); + q[i] = fourD / 4.0; + q[j] = (m[j][i] + m[i][j]) / fourD; + q[k] = (m[k][i] + m[i][k]) / fourD; + q[3] = (m[j][k] - m[k][j]) / fourD; + } + + Quat.x = q[0]; + Quat.y = q[1]; + Quat.z = q[2]; + Quat.w = q[3]; + return Quat; +} +void Quat_2_Matrix(quaternion Quat, Matrix_t m) +{ + // From the GLVelocity site (http://glvelocity.gamedev.net) + float fW = Quat.w; + float fX = Quat.x; + float fY = Quat.y; + float fZ = Quat.z; + float fXX = fX * fX; + float fYY = fY * fY; + float fZZ = fZ * fZ; + m[0][0] = 1.0f - 2.0f * (fYY + fZZ); + m[1][0] = 2.0f * (fX * fY + fW * fZ); + m[2][0] = 2.0f * (fX * fZ - fW * fY); + m[3][0] = 0.0f; + m[0][1] = 2.0f * (fX * fY - fW * fZ); + m[1][1] = 1.0f - 2.0f * (fXX + fZZ); + m[2][1] = 2.0f * (fY * fZ + fW * fX); + m[3][1] = 0.0f; + m[0][2] = 2.0f * (fX * fZ + fW * fY); + m[1][2] = 2.0f * (fX * fZ - fW * fX); + m[2][2] = 1.0f - 2.0f * (fXX + fYY); + m[3][2] = 0.0f; + m[0][3] = 0.0f; + m[1][3] = 0.0f; + m[2][3] = 0.0f; + m[3][3] = 1.0f; +} +quaternion To_Quat(angle_axis Ang_Ax) +{ + // From the Quaternion Powers article on gamedev.net + quaternion Quat; + + Quat.x = Ang_Ax.x * sin(Ang_Ax.angle / 2); + Quat.y = Ang_Ax.y * sin(Ang_Ax.angle / 2); + Quat.z = Ang_Ax.z * sin(Ang_Ax.angle / 2); + Quat.w = cos(Ang_Ax.angle / 2); + return Quat; +} +angle_axis Quat_2_AA(quaternion Quat) +{ + angle_axis Ang_Ax; + float scale, tw; + tw = (float)acos(Quat.w) * 2; + scale = (float)sin(tw / 2.0); + Ang_Ax.x = Quat.x / scale; + Ang_Ax.y = Quat.y / scale; + Ang_Ax.z = Quat.z / scale; + + Ang_Ax.angle = 2.0 * acos(Quat.w)/(float)PI*180; + return Ang_Ax; +} + +quaternion To_Quat(int In_Degrees, euler Euler) +{ + // From the gamasutra quaternion article + quaternion Quat; + float cr, cp, cy, sr, sp, sy, cpcy, spsy; + //If we are in Degree mode, convert to Radians + if (In_Degrees) { + Euler.x = Euler.x * (float)PI / 180; + Euler.y = Euler.y * (float)PI / 180; + Euler.z = Euler.z * (float)PI / 180; + } + //Calculate trig identities + //Formerly roll, pitch, yaw + cr = float(cos(Euler.x/2)); + cp = float(cos(Euler.y/2)); + cy = float(cos(Euler.z/2)); + sr = float(sin(Euler.x/2)); + sp = float(sin(Euler.y/2)); + sy = float(sin(Euler.z/2)); + + cpcy = cp * cy; + spsy = sp * sy; + Quat.w = cr * cpcy + sr * spsy; + Quat.x = sr * cpcy - cr * spsy; + Quat.y = cr * sp * cy + sr * cp * sy; + Quat.z = cr * cp * sy - sr * sp * cy; + + return Quat; +} + +quaternion QNormalize(quaternion Quat) +{ + float norm; + norm = Quat.x * Quat.x + + Quat.y * Quat.y + + Quat.z * Quat.z + + Quat.w * Quat.w; + Quat.x = float(Quat.x / norm); + Quat.y = float(Quat.y / norm); + Quat.z = float(Quat.z / norm); + Quat.w = float(Quat.w / norm); + return Quat; +} + +XYZ Quat2Vector(quaternion Quat) +{ + QNormalize(Quat); + + float fW = Quat.w; + float fX = Quat.x; + float fY = Quat.y; + float fZ = Quat.z; + + XYZ tempvec; + + tempvec.x = 2.0f*(fX*fZ-fW*fY); + tempvec.y = 2.0f*(fY*fZ+fW*fX); + tempvec.z = 1.0f-2.0f*(fX*fX+fY*fY); + + return tempvec; +} + +void CrossProduct(XYZ P, XYZ Q, XYZ *V){ + V->x = P.y * Q.z - P.z * Q.y; + V->y = P.z * Q.x - P.x * Q.z; + V->z = P.x * Q.y - P.y * Q.x; +} + +void Normalise(XYZ *vectory) { + float d = fast_sqrt(vectory->x*vectory->x+vectory->y*vectory->y+vectory->z*vectory->z); + if(d==0){return;} + vectory->x /= d; + vectory->y /= d; + vectory->z /= d; +} + +float fast_sqrt (register float arg) +{ +#ifdef OS9 + // Can replace with slower return std::sqrt(arg); + register float result; + + if (arg == 0.0) return 0.0; + + asm { + frsqrte result,arg // Calculate Square root + } + + // Newton Rhapson iterations. + result = result + 0.5 * result * (1.0 - arg * result * result); + result = result + 0.5 * result * (1.0 - arg * result * result); + + return result * arg; +#else + return sqrt(arg); +#endif +} + +float normaldotproduct(XYZ point1, XYZ point2){ + GLfloat returnvalue; + Normalise(&point1); + Normalise(&point2); + returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z); + return returnvalue; +} + +extern float u0, u1, u2; +extern float v0, v1, v2; +extern float a, b; +extern int i, j; +extern bool bInter; +extern float pointv[3]; +extern float p1v[3]; +extern float p2v[3]; +extern float p3v[3]; +extern float normalv[3]; + +bool PointInTriangle(Vector *p, Vector normal, float p11, float p12, float p13, float p21, float p22, float p23, float p31, float p32, float p33) +{ + bInter=0; + + pointv[0]=p->x; + pointv[1]=p->y; + pointv[2]=p->z; + + + p1v[0]=p11; + p1v[1]=p12; + p1v[2]=p13; + + p2v[0]=p21; + p2v[1]=p22; + p2v[2]=p23; + + p3v[0]=p31; + p3v[1]=p32; + p3v[2]=p33; + + normalv[0]=normal.x; + normalv[1]=normal.y; + normalv[2]=normal.z; + +#define ABS(X) (((X)<0.f)?-(X):(X) ) +#define MAX(A, B) (((A)<(B))?(B):(A)) + float max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2])); +#undef MAX + if (max == ABS(normalv[0])) {i = 1; j = 2;} // y, z + if (max == ABS(normalv[1])) {i = 0; j = 2;} // x, z + if (max == ABS(normalv[2])) {i = 0; j = 1;} // x, y +#undef ABS + + u0 = pointv[i] - p1v[i]; + v0 = pointv[j] - p1v[j]; + u1 = p2v[i] - p1v[i]; + v1 = p2v[j] - p1v[j]; + u2 = p3v[i] - p1v[i]; + v2 = p3v[j] - p1v[j]; + + if (u1 > -1.0e-05f && u1 < 1.0e-05f)// == 0.0f) + { + b = u0 / u2; + if (0.0f <= b && b <= 1.0f) + { + a = (v0 - b * v2) / v1; + if ((a >= 0.0f) && (( a + b ) <= 1.0f)) + bInter = 1; + } + } + else + { + b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1); + if (0.0f <= b && b <= 1.0f) + { + a = (u0 - b * u2) / u1; + if ((a >= 0.0f) && (( a + b ) <= 1.0f )) + bInter = 1; + } + } + + return bInter; +} + +bool LineFacet(Vector p1,Vector p2,Vector pa,Vector pb,Vector pc,Vector *p) +{ + float d; + float a1,a2,a3; + float total,denom,mu; + Vector n,pa1,pa2,pa3; + + //Calculate the parameters for the plane + n.x = (pb.y - pa.y)*(pc.z - pa.z) - (pb.z - pa.z)*(pc.y - pa.y); + n.y = (pb.z - pa.z)*(pc.x - pa.x) - (pb.x - pa.x)*(pc.z - pa.z); + n.z = (pb.x - pa.x)*(pc.y - pa.y) - (pb.y - pa.y)*(pc.x - pa.x); + n.Normalize(); + d = - n.x * pa.x - n.y * pa.y - n.z * pa.z; + + //Calculate the position on the line that intersects the plane + denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z); + if (abs(denom) < 0.0000001) // Line and plane don't intersect + return 0; + mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom; + p->x = p1.x + mu * (p2.x - p1.x); + p->y = p1.y + mu * (p2.y - p1.y); + p->z = p1.z + mu * (p2.z - p1.z); + if (mu < 0 || mu > 1) // Intersection not along line segment + return 0; + + if(!PointInTriangle( p, n, pa.x, pa.y, pa.z, pb.x, pb.y, pb.z, pc.x, pc.y, pc.z)){return 0;} + + return 1; +} + +bool PointInTriangle(XYZ *p, XYZ normal, XYZ *p1, XYZ *p2, XYZ *p3) +{ + bInter=0; + + pointv[0]=p->x; + pointv[1]=p->y; + pointv[2]=p->z; + + + p1v[0]=p1->x; + p1v[1]=p1->y; + p1v[2]=p1->z; + + p2v[0]=p2->x; + p2v[1]=p2->y; + p2v[2]=p2->z; + + p3v[0]=p3->x; + p3v[1]=p3->y; + p3v[2]=p3->z; + + normalv[0]=normal.x; + normalv[1]=normal.y; + normalv[2]=normal.z; + +#define ABS(X) (((X)<0.f)?-(X):(X) ) +#define MAX(A, B) (((A)<(B))?(B):(A)) + float max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2])); +#undef MAX + if (max == ABS(normalv[0])) {i = 1; j = 2;} // y, z + if (max == ABS(normalv[1])) {i = 0; j = 2;} // x, z + if (max == ABS(normalv[2])) {i = 0; j = 1;} // x, y +#undef ABS + + u0 = pointv[i] - p1v[i]; + v0 = pointv[j] - p1v[j]; + u1 = p2v[i] - p1v[i]; + v1 = p2v[j] - p1v[j]; + u2 = p3v[i] - p1v[i]; + v2 = p3v[j] - p1v[j]; + + if (u1 > -1.0e-05f && u1 < 1.0e-05f)// == 0.0f) + { + b = u0 / u2; + if (0.0f <= b && b <= 1.0f) + { + a = (v0 - b * v2) / v1; + if ((a >= 0.0f) && (( a + b ) <= 1.0f)) + bInter = 1; + } + } + else + { + b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1); + if (0.0f <= b && b <= 1.0f) + { + a = (u0 - b * u2) / u1; + if ((a >= 0.0f) && (( a + b ) <= 1.0f )) + bInter = 1; + } + } + + return bInter; +} + +bool LineFacet(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p) +{ + float d; + float a1,a2,a3; + float total,denom,mu; + XYZ n,pa1,pa2,pa3; + + //Calculate the parameters for the plane + n.x = (pb.y - pa.y)*(pc.z - pa.z) - (pb.z - pa.z)*(pc.y - pa.y); + n.y = (pb.z - pa.z)*(pc.x - pa.x) - (pb.x - pa.x)*(pc.z - pa.z); + n.z = (pb.x - pa.x)*(pc.y - pa.y) - (pb.y - pa.y)*(pc.x - pa.x); + Normalise(&n); + d = - n.x * pa.x - n.y * pa.y - n.z * pa.z; + + //Calculate the position on the line that intersects the plane + denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z); + if (abs(denom) < 0.0000001) // Line and plane don't intersect + return 0; + mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom; + p->x = p1.x + mu * (p2.x - p1.x); + p->y = p1.y + mu * (p2.y - p1.y); + p->z = p1.z + mu * (p2.z - p1.z); + if (mu < 0 || mu > 1) // Intersection not along line segment + return 0; + + if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;} + + return 1; +} + +extern float d; +extern float a1,a2,a3; +extern float total,denom,mu; +extern XYZ pa1,pa2,pa3,n; + +float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc,XYZ *p) +{ + + //Calculate the parameters for the plane + n.x = (pb.y - pa.y)*(pc.z - pa.z) - (pb.z - pa.z)*(pc.y - pa.y); + n.y = (pb.z - pa.z)*(pc.x - pa.x) - (pb.x - pa.x)*(pc.z - pa.z); + n.z = (pb.x - pa.x)*(pc.y - pa.y) - (pb.y - pa.y)*(pc.x - pa.x); + Normalise(&n); + d = - n.x * pa.x - n.y * pa.y - n.z * pa.z; + + //Calculate the position on the line that intersects the plane + denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z); + if (abs(denom) < 0.0000001) // Line and plane don't intersect + return 0; + mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom; + p->x = p1.x + mu * (p2.x - p1.x); + p->y = p1.y + mu * (p2.y - p1.y); + p->z = p1.z + mu * (p2.z - p1.z); + if (mu < 0 || mu > 1) // Intersection not along line segment + return 0; + + if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;} + + return 1; +} + +float LineFacetd(XYZ p1,XYZ p2,XYZ pa,XYZ pb,XYZ pc, XYZ n, XYZ *p) +{ + + //Calculate the parameters for the plane + d = - n.x * pa.x - n.y * pa.y - n.z * pa.z; + + //Calculate the position on the line that intersects the plane + denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z); + if (abs(denom) < 0.0000001) // Line and plane don't intersect + return 0; + mu = - (d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom; + p->x = p1.x + mu * (p2.x - p1.x); + p->y = p1.y + mu * (p2.y - p1.y); + p->z = p1.z + mu * (p2.z - p1.z); + if (mu < 0 || mu > 1) // Intersection not along line segment + return 0; + + if(!PointInTriangle( p, n, &pa, &pb, &pc)){return 0;} + return 1; +} + +float LineFacetd(XYZ *p1,XYZ *p2,XYZ *pa,XYZ *pb,XYZ *pc, XYZ *n, XYZ *p) +{ + + //Calculate the parameters for the plane + d = - n->x * pa->x - n->y * pa->y - n->z * pa->z; + + //Calculate the position on the line that intersects the plane + denom = n->x * (p2->x - p1->x) + n->y * (p2->y - p1->y) + n->z * (p2->z - p1->z); + if (abs(denom) < 0.0000001) // Line and plane don't intersect + return 0; + mu = - (d + n->x * p1->x + n->y * p1->y + n->z * p1->z) / denom; + p->x = p1->x + mu * (p2->x - p1->x); + p->y = p1->y + mu * (p2->y - p1->y); + p->z = p1->z + mu * (p2->z - p1->z); + if (mu < 0 || mu > 1) // Intersection not along line segment + return 0; + + if(!PointInTriangle( p, *n, pa, pb, pc)){return 0;} + return 1; +} + +void ReflectVector(XYZ *vel, XYZ *n) +{ + XYZ vn; + XYZ vt; + float dotprod; + + dotprod=dotproduct(*n,*vel); + vn.x=n->x*dotprod; + vn.y=n->y*dotprod; + vn.z=n->z*dotprod; + + vt.x=vel->x-vn.x; + vt.y=vel->y-vn.y; + vt.z=vel->z-vn.z; + + vel->x = vt.x - vn.x; + vel->y = vt.y - vn.y; + vel->z = vt.z - vn.z; +} + +float dotproduct(XYZ point1, XYZ point2){ + GLfloat returnvalue; + returnvalue=(point1.x*point2.x+point1.y*point2.y+point1.z*point2.z); + return returnvalue; +} + +float findDistance(XYZ point1, XYZ point2){ + return(fast_sqrt((point1.x-point2.x)*(point1.x-point2.x)+(point1.y-point2.y)*(point1.y-point2.y)+(point1.z-point2.z)*(point1.z-point2.z))); +} + +float findLength(XYZ point1){ + return(fast_sqrt((point1.x)*(point1.x)+(point1.y)*(point1.y)+(point1.z)*(point1.z))); +} + + +float findLengthfast(XYZ point1){ + return((point1.x)*(point1.x)+(point1.y)*(point1.y)+(point1.z)*(point1.z)); +} + +float findDistancefast(XYZ point1, XYZ point2){ + return((point1.x-point2.x)*(point1.x-point2.x)+(point1.y-point2.y)*(point1.y-point2.y)+(point1.z-point2.z)*(point1.z-point2.z)); +} + +XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang){ + XYZ newpoint; + if(xang){ + xang*=6.283185; + xang/=360; + } + if(yang){ + yang*=6.283185; + yang/=360; + } + if(zang){ + zang*=6.283185; + zang/=360; + } + + + if(yang){ + newpoint.z=thePoint.z*cos(yang)-thePoint.x*sin(yang); + newpoint.x=thePoint.z*sin(yang)+thePoint.x*cos(yang); + thePoint.z=newpoint.z; + thePoint.x=newpoint.x; + } + + if(zang){ + newpoint.x=thePoint.x*cos(zang)-thePoint.y*sin(zang); + newpoint.y=thePoint.y*cos(zang)+thePoint.x*sin(zang); + thePoint.x=newpoint.x; + thePoint.y=newpoint.y; + } + + if(xang){ + newpoint.y=thePoint.y*cos(xang)-thePoint.z*sin(xang); + newpoint.z=thePoint.y*sin(xang)+thePoint.z*cos(xang); + thePoint.z=newpoint.z; + thePoint.y=newpoint.y; + } + + return thePoint; +} + +float square( float f ) { return (f*f) ;} + +bool sphere_line_intersection ( + float x1, float y1 , float z1, + float x2, float y2 , float z2, + float x3, float y3 , float z3, float r ) +{ + + // x1,y1,z1 P1 coordinates (point of line) + // x2,y2,z2 P2 coordinates (point of line) + // x3,y3,z3, r P3 coordinates and radius (sphere) + // x,y,z intersection coordinates + // + // This function returns a pointer array which first index indicates + // the number of intersection point, followed by coordinate pairs. + + float x , y , z; + float a, b, c, mu, i ; + + if(x1>x3+r&&x2>x3+r)return(0); + if(x1<x3-r&&x2<x3-r)return(0); + if(y1>y3+r&&y2>y3+r)return(0); + if(y1<y3-r&&y2<y3-r)return(0); + if(z1>z3+r&&z2>z3+r)return(0); + if(z1<z3-r&&z2<z3-r)return(0); + a = square(x2 - x1) + square(y2 - y1) + square(z2 - z1); + b = 2* ( (x2 - x1)*(x1 - x3) + + (y2 - y1)*(y1 - y3) + + (z2 - z1)*(z1 - z3) ) ; + c = square(x3) + square(y3) + + square(z3) + square(x1) + + square(y1) + square(z1) - + 2* ( x3*x1 + y3*y1 + z3*z1 ) - square(r) ; + i = b * b - 4 * a * c ; + + if ( i < 0.0 ) + { + // no intersection + return(0); + } + + return(1); +} + +XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang){ + XYZ newpoint; + XYZ oldpoint; + + oldpoint=thePoint; + + if(yang!=0){ + newpoint.z=oldpoint.z*cos(yang)-oldpoint.x*sin(yang); + newpoint.x=oldpoint.z*sin(yang)+oldpoint.x*cos(yang); + oldpoint.z=newpoint.z; + oldpoint.x=newpoint.x; + } + + if(zang!=0){ + newpoint.x=oldpoint.x*cos(zang)-oldpoint.y*sin(zang); + newpoint.y=oldpoint.y*cos(zang)+oldpoint.x*sin(zang); + oldpoint.x=newpoint.x; + oldpoint.y=newpoint.y; + } + + if(xang!=0){ + newpoint.y=oldpoint.y*cos(xang)-oldpoint.z*sin(xang); + newpoint.z=oldpoint.y*sin(xang)+oldpoint.z*cos(xang); + oldpoint.z=newpoint.z; + oldpoint.y=newpoint.y; + } + + return oldpoint; + +} + |