I am following the quaternion tutorial: http://www.raywenderlich.com/12667/how-to-rotate-a-3d-object-using-touches-with-opengl and am trying to rotate a globe to some XYZ location. I have an initial quaternion and generate a random XYZ location on the surface of the globe. I pass that XYZ location into the following function. The idea was to generate a lookAt vector with GLKMatrix4MakeLookAt and define the end Quaternion for the slerp step from the lookAt matrix.
- (void)rotateToLocationX:(float)x andY:(float)y andZ:(float)z {
// Turn on the interpolation for smooth rotation
_slerping = YES; // Begin auto rotating to this location
_slerpCur = 0;
_slerpMax = 1.0;
_slerpStart = _quat;
// The eye location is defined by the look at location multiplied by this modifier
float modifier = 1.0;
// Create a look at vector for which we will create a GLK4Matrix from
float xEye = x;
float yEye = y;
float zEye = z;
//NSLog(#"%f %f %f %f %f %f",xEye, yEye, zEye, x, y, z);
_currentSatelliteLocation = GLKMatrix4MakeLookAt(xEye, yEye, zEye, 0, 0, 0, 0, 1, 0);
_currentSatelliteLocation = GLKMatrix4Multiply(_currentSatelliteLocation,self.effect.transform.modelviewMatrix);
// Turn our 4x4 matrix into a quat and use it to mark the end point of our interpolation
//_currentSatelliteLocation = GLKMatrix4Translate(_currentSatelliteLocation, 0.0f, 0.0f, GLOBAL_EARTH_Z_LOCATION);
_slerpEnd = GLKQuaternionMakeWithMatrix4(_currentSatelliteLocation);
// Print info on the quat
GLKVector3 vec = GLKQuaternionAxis(_slerpEnd);
float angle = GLKQuaternionAngle(_slerpEnd);
//NSLog(#"%f %f %f %f",vec.x,vec.y,vec.z,angle);
NSLog(#"Quat end:");
[self printMatrix:_currentSatelliteLocation];
//[self printMatrix:self.effect.transform.modelviewMatrix];
}
The interpolation works, I get a smooth rotation, however the ending location is never the XYZ I input - I know this because my globe is a sphere and I am calculating XYZ from Lat Lon. I want to look directly down the 'lookAt' vector toward the center of the earth from that lat/lon location on the surface of the globe after the rotation. I think it may have something to do with the up vector but I've tried everything that made sense.
What am I doing wrong - How can I define a final quaternion that when I finish rotating, looks down a vector to the XYZ on the surface of the globe? Thanks!
Is the following your meaning:
Your globe center is (0, 0, 0), radius is R, the start position is (0, 0, R), your final position is (0, R, 0), so rotate the globe 90 degrees around X-asix?
If so, just set lookat function eye position to your final position, the look at parameters to the globe center.
m_target.x = 0.0f;
m_target.y = 0.0f;
m_target.z = 1.0f;
m_right.x = 1.0f;
m_right.y = 0.0f;
m_right.z = 0.0f;
m_up.x = 0.0f;
m_up.y = 1.0f;
m_up.z = 0.0f;
void CCamera::RotateX( float amount )
{
Point3D target = m_target;
Point3D up = m_up;
amount = amount / 180 * PI;
m_target.x = (cos(PI / 2 - amount) * up.x) + (cos(amount) * target.x);
m_target.y = (cos(PI / 2 - amount) * up.y) + (cos(amount) * target.y);
m_target.z = (cos(PI / 2 - amount) * up.z) + (cos(amount) * target.z);
m_up.x = (cos(amount) * up.x) + (cos(PI / 2 + amount) * target.x);
m_up.y = (cos(amount) * up.y) + (cos(PI / 2 + amount) * target.y);
m_up.z = (cos(amount) * up.z) + (cos(PI / 2 + amount) * target.z);
Normalize(m_target);
Normalize(m_up);
}
void CCamera::RotateY( float amount )
{
Point3D target = m_target;
Point3D right = m_right;
amount = amount / 180 * PI;
m_target.x = (cos(PI / 2 + amount) * right.x) + (cos(amount) * target.x);
m_target.y = (cos(PI / 2 + amount) * right.y) + (cos(amount) * target.y);
m_target.z = (cos(PI / 2 + amount) * right.z) + (cos(amount) * target.z);
m_right.x = (cos(amount) * right.x) + (cos(PI / 2 - amount) * target.x);
m_right.y = (cos(amount) * right.y) + (cos(PI / 2 - amount) * target.y);
m_right.z = (cos(amount) * right.z) + (cos(PI / 2 - amount) * target.z);
Normalize(m_target);
Normalize(m_right);
}
void CCamera::RotateZ( float amount )
{
Point3D right = m_right;
Point3D up = m_up;
amount = amount / 180 * PI;
m_up.x = (cos(amount) * up.x) + (cos(PI / 2 - amount) * right.x);
m_up.y = (cos(amount) * up.y) + (cos(PI / 2 - amount) * right.y);
m_up.z = (cos(amount) * up.z) + (cos(PI / 2 - amount) * right.z);
m_right.x = (cos(PI / 2 + amount) * up.x) + (cos(amount) * right.x);
m_right.y = (cos(PI / 2 + amount) * up.y) + (cos(amount) * right.y);
m_right.z = (cos(PI / 2 + amount) * up.z) + (cos(amount) * right.z);
Normalize(m_right);
Normalize(m_up);
}
void CCamera::Normalize( Point3D &p )
{
float length = sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
if (1 == length || 0 == length)
{
return;
}
float scaleFactor = 1.0 / length;
p.x *= scaleFactor;
p.y *= scaleFactor;
p.z *= scaleFactor;
}
The answer to this question is a combination of the following rotateTo function and a change to the code from Ray's tutorial at ( http://www.raywenderlich.com/12667/how-to-rotate-a-3d-object-using-touches-with-opengl ). As one of the comments on that article says there is an arbitrary factor of 2.0 being multiplied in GLKQuaternion Q_rot = GLKQuaternionMakeWithAngleAndVector3Axis(angle * 2.0, axis);. Remove that "2" and use the following function to create the _slerpEnd - after that the globe will rotate smoothly to XYZ specified.
// Rotate the globe using Slerp interpolation to an XYZ coordinate
- (void)rotateToLocationX:(float)x andY:(float)y andZ:(float)z {
// Turn on the interpolation for smooth rotation
_slerping = YES; // Begin auto rotating to this location
_slerpCur = 0;
_slerpMax = 1.0;
_slerpStart = _quat;
// Create a look at vector for which we will create a GLK4Matrix from
float xEye = x;
float yEye = y;
float zEye = z;
_currentSatelliteLocation = GLKMatrix4MakeLookAt(xEye, yEye, zEye, 0, 0, 0, 0, 1, 0);
// Turn our 4x4 matrix into a quat and use it to mark the end point of our interpolation
_slerpEnd = GLKQuaternionMakeWithMatrix4(_currentSatelliteLocation);
}
I am facing a typical problem in rotating an object. Description is as given below
I have taken two CGPoint let say point1 and point2
point1 = (50,50)
point2 = (150, 50)
this point will draw a horizontal line.
Now i am drawing a rectangle with that point on it. Width is 100 and height is 10. Angle is 0.see screen shot
works fine
now i change the point let say
point1 = (50,50)
point2 = (50,150)
this point will draw a vertical line.
For rectangle Angle is 90. With this point rectangle is not drawing properlysee screen shot
My code for drawing rectangle is :
CGPoint mid = CGPointMake((point1.x+point2.x)/2, (point1.y+point2.y)/2)
CGPoint UL = CGPointMake(mid.x + ( Width / 2 ) * cos (A) - ( Height / 2 ) * sin (A) , mid.y + ( Height / 2 ) * cos (A) + ( Width / 2 ) * sin (A));
CGContextMoveToPoint(context, UL.x,routeView.frame.size.height - UL.y);
CGPoint UR = CGPointMake(mid.x - ( Width / 2 ) * cos (A) - ( Height / 2 ) * sin (A) , mid.y + ( Height / 2 ) * cos (A) - ( Width / 2 ) * sin (A));
CGContextAddLineToPoint(context, UR.x,routeView.frame.size.height - UR.y);
CGPoint BR = CGPointMake(mid.x - ( Width / 2 ) * cos (A) + ( Height / 2 ) * sin (A) , mid.y - ( Height / 2 ) * cos (A) - ( Width / 2 ) * sin (A));
CGContextAddLineToPoint(context, BR.x,routeView.frame.size.height - BR.y);
CGPoint BL = CGPointMake(mid.x + ( Width / 2 ) * cos (A) + ( Height / 2 ) * sin (A) , mid.y - ( Height / 2 ) * cos (A) + ( Width / 2 ) * sin (A));
CGContextAddLineToPoint(context, BL.x,routeView.frame.size.height - BL.y);
CGContextAddLineToPoint(context, UL.x,routeView.frame.size.height - UL.y);
CGContextStrokePath(context);
Here A is Angle and it is not static, mid is middle point of point1 and point2
for more ref see this
Am I missing something?
Please help me if you have any idea.......
Thanks,
Let me guess, it's actually rotated about 26 degrees too far, right?
(90 x 180) / Pi ~= 5156.62 = (360 x 14) + 90 + 26.62
You rotated it 90 radians by mistake.
I have this code:
CGPoint arrowMiddle = CGPointMake((arrowOne.x + arrowTo.x)/2, (arrowOne.y + arrowTo.y)/2);
CGPoint arrowLeft = CGPointMake(arrowMiddle.x-40, arrowMiddle.y);
CGPoint arrowRight = CGPointMake(arrowMiddle.x, arrowMiddle.y + 40);
[arrowPath addLineToScreenPoint:arrowLeft];
[arrowPath addLineToScreenPoint:arrowMiddle];
[arrowPath addLineToScreenPoint:arrowRight];
[[mapContents overlay] addSublayer:arrowPath];
[arrowPath release];
with this output:
http://img517.yfrog.com/img517/7690/schermafbeelding2010032.png
What have i to add to get the left and right the at same degree of the line + 30°.
If someone has the algorithm of drawing an arrow on a line, pleas give it. It doesn't matter what programming language it is...
Thanks
Here is what you do. First, take the vector of the line and normalize it by dividing it by its length — this will give you a vector of length 1 pointing in the direction of the line. Next, multiply it by the length you need it to be. Turn it by 120° and -120° to make the arrow. Finally, offset it by the coordinates where you want it to be. Here is how it would look like in code:
// calculate the position of the arrow
CGPoint arrowMiddle;
arrowMiddle.x = (arrowOne.x + arrowTo.x) / 2;
arrowMiddle.y = (arrowOne.y + arrowTo.y) / 2;
// create a line vector
CGPoint v;
v.x = arrowTo.x - arrowOne.x;
v.y = arrowTo.y - arrowOne.y;
// normalize it and multiply by needed length
CGFloat length = sqrt(v.x * v.x + v.y * v.y);
v.x = 40 * (v.x / length);
v.y = 40 * (v.y / length);
// turn it by 120° and offset to position
CGPoint arrowLeft = CGPointApplyAffineTransform(v, CGAffineTransformMakeRotation(3.14 * 2 / 3));
arrowLeft.x = arrowLeft.x + arrowMiddle.x;
arrowLeft.y = arrowLeft.y + arrowMiddle.y;
// turn it by -120° and offset to position
CGPoint arrowRight = CGPointApplyAffineTransform(v, CGAffineTransformMakeRotation(-3.14 * 2 / 3));
arrowRight.x = arrowRight.x + arrowMiddle.x;
arrowRight.y = arrowRight.y + arrowMiddle.y;
Thanks for respond!
In the meanwhile I found also an solution.
It's Like this:
double slopy , cosy , siny;
double Par = 10.0; //length of Arrow (>)
slopy = atan2( ( arrowOne.y - arrowTo.y ),
( arrowOne.x - arrowTo.x ) );
cosy = cos( slopy );
siny = sin( slopy ); //need math.h for these functions
CGPoint arrowMiddle = CGPointMake((arrowOne.x + arrowTo.x)/2, (arrowOne.y + arrowTo.y)/2);
[arrowPath addLineToScreenPoint:arrowMiddle];
CGPoint arrowLeft = CGPointMake( arrowMiddle.x + round( - Par * cosy - ( Par / 2.0 * siny ) ), arrowMiddle.y + round( - Par * siny + ( Par / 2.0 * cosy ) ) );
[arrowPath addLineToScreenPoint:arrowLeft];
CGPoint arrowRight = CGPointMake( arrowMiddle.x + round( - Par * cosy + ( Par / 2.0 * siny ) ),arrowMiddle.y - round( Par / 2.0 * cosy + Par * siny ) );
[arrowPath addLineToScreenPoint:arrowRight];
[arrowPath addLineToScreenPoint:arrowMiddle];
[[mapContents overlay] addSublayer:arrowPath];
[arrowPath release];
The only problem here is that i draw it like it's an RMPath(route-me framework) and that the arrow gets bigger/smaller when you zoom in/out.
But thanks for respond, I will look into it which code is the most perform.
Looking to do classic OpenGL mouse picking in ES. I'd prefer not to use third party libs, GLU ports and OpenGL name stacks, etc, are out. This pretty much leaves inverse view transformation and ray intersection, correct?
I've gotten pretty far with the help of:
http://trac.bookofhook.com/bookofhook/trac.cgi/wiki/MousePicking
http://eigenclass.blogspot.com/2008/10/opengl-es-picking-using-ray-boundingbox.html
. . .but I'm not there yet. This also reeks of THERE MUST BE AN EASIER WAY!!
Here is some code:
-(void)handleTouch:(CGPoint)point {
GLfloat width = backingWidth;
GLfloat height = backingHeight;
GLfloat x = point.x;
GLfloat y = point.y;
GLfloat z = 0.0f;
//viewport -> normalized dev coord -> clip
GLfloat n[] = {
2 * x / width - 1,
2 * y / height,
2 * z - 1,
1
};
float fov = 45.0f * (M_PI / 180.0f);
float near = 0.01, far = 10.0f;
float aspect = (float)backingWidth / (float)backingHeight;
float top = tan(fov) * near;
//float bottom = -top;
//float left = aspect * bottom;
float right = aspect * top;
//I'm a viewing volume symmetric projection matrix
GLfloat P[] = {
near / right, 0, 0, 0,
0, near / top, 0, 0,
0, 0, -(far + near) / (far - near), (-2 * far * near) / (far - near),
0, 0, -1, 0
};
GLfloat Pminus1[] = {
1/P[0], 0, 0, 0,
0, 1/P[5], 0, 0,
0, 0, 0, 1/P[14],
0, 0, 1/P[11], -(P[10]/ (P[11]*P[14]))
};
//clip -> view
GLfloat v[] = {
Pminus1[0] * n[0] + Pminus1[1] * n[1] + Pminus1[2] * n[2] + Pminus1[3] * n[3],
Pminus1[4] * n[0] + Pminus1[5] * n[1] + Pminus1[6] * n[2] + Pminus1[7] * n[3],
Pminus1[8] * n[0] + Pminus1[9] * n[1] + Pminus1[10] * n[2] + Pminus1[11] * n[3],
Pminus1[12] * n[0] + Pminus1[13] * n[1] + Pminus1[14] * n[2] + Pminus1[15] * n[3]
};
//view -> world
GLfloat Rt[] = {
mv[0], mv[4], mv[8],
mv[1], mv[5], mv[9],
mv[2], mv[6], mv[10]
};
GLfloat tPrime[] = {
Rt[0] * mv[3] + Rt[1] * mv[7] + Rt[2] * mv[11],
Rt[3] * mv[3] + Rt[4] * mv[7] + Rt[5] * mv[11],
Rt[6] * mv[3] + Rt[7] * mv[7] + Rt[8] * mv[11]
};
GLfloat Mminus1[] = {
Rt[0], Rt[1], Rt[2], -(tPrime[0]),
Rt[3], Rt[4], Rt[5], -(tPrime[1]),
Rt[6], Rt[7], Rt[8], -(tPrime[2]),
0, 0, 0, 1
};
//point in world space
GLfloat w[] = {
Mminus1[0] * v[0] + Mminus1[1] * v[1] + Mminus1[2] * v[2] + Mminus1[3] * v[3],
Mminus1[4] * v[0] + Mminus1[5] * v[1] + Mminus1[6] * v[2] + Mminus1[7] * v[3],
Mminus1[8] * v[0] + Mminus1[9] * v[1] + Mminus1[10] * v[2] + Mminus1[11] * v[3],
Mminus1[12] * v[0] + Mminus1[13] * v[1] + Mminus1[14] * v[2] + Mminus1[15] * v[3]
};
//r = a + t(w - a)
GLfloat a[] = {0.0f, -0.1f, 0.0f};
GLfloat wminusa[] = {w[0] - a[0], w[1] - a[1], w[2] - a[2]};
vector[0] = a[0];
vector[1] = a[1],
vector[2] = a[2];
vector[3] = w[0];
vector[4] = w[1];
vector[5] = -10.0f;
//3 non-colinear points on the plane
GLfloat p1[] = {rect.origin.x, rect.origin.y, 0};
GLfloat p2[] = {rect.origin.x + rect.size.width, rect.origin.y, 0};
GLfloat p3[] = {rect.origin.x + rect.size.width, rect.origin.y + rect.size.height, 0};
//location plane normal vector, Ax + By + Cz + D = 0
GLfloat lp[] = {
p1[1] * (p2[2] - p3[2]) + p2[1] * (p3[2] - p1[2]) + p3[1] * (p1[2] - p2[2]),
p1[2] * (p2[0] - p3[0]) + p2[2] * (p3[0] - p1[0]) + p3[2] * (p1[0] - p2[0]),
p1[0] * (p2[1] - p3[1]) + p2[0] * (p3[1] - p1[1]) + p3[0] * (p1[1] - p2[1]),
-(p1[0] * (((p2[1] * p3[2]) - (p3[1] * p2[2]))) + p2[0] * (((p3[1] * p1[2]) - (p1[1] * p3[2]))) + p3[0] * (((p1[1] * p2[2]) - (p2[1] * p1[2]))))
};
GLfloat PnRd = (lp[0] * wminusa[0]) + (lp[1] * wminusa[1]) + (lp[2] * wminusa[2]);
if(PnRd != 0) {
GLfloat PnR0D = -((lp[0] * a[0]) + (lp[1] * a[1]) + (lp[2] * a[2]) + lp[3]);
if(PnR0D != 0) {
GLfloat t = PnR0D / PnRd;
if(t >= 0) {
GLfloat p[] = {
a[0] + wminusa[0] * t,
a[1] + wminusa[1] * t,
a[2] + wminusa[2] * t
};
if(p[0] > rect.origin.x &&
p[0] < rect.origin.x + rect.size.width &&
p[1] > rect.origin.y &&
p[1] < rect.origin.y + rect.size.height)
NSLog(#"BOOM!!!");
}
}
}
}
This post is very hard to follow. I'm attempting to roll my own on iOS 5 with GLKView; I've worked out how to touch detect pixel RGBA as I describe here, now I'm trying to work out how to quickly change the colours of my scene objects to be unique, to accompany this method.
I managed to fix it:
-(void)view2WorldPoint:(CGPoint)point :(GLfloat*)worldPoint {
// this is the inverse translation of the modelview
GLfloat width = (GLfloat)backingWidth;
GLfloat height = (GLfloat)backingHeight;
float clickX = point.x;
float clickY = point.y;
float clickZ = 0.0f;
NSLog(#"click point : x = %f, y = %f, z = %f", clickX, clickY, clickZ);
// NSLog(#"Me : x = %f, y = %f, z = %f", a[0], a[1], a[2]);
// NSLog(#"Dev : x = %f, y = %f, z = %f", squareX, squareY, squareZ);
//viewport -> normalized device coord -> clip
GLfloat n[] = {
2 * clickX / width - 1,
2 * (480-clickY) / height - 1,
2 * clickZ - 1,
1
};
// NSLog(#"Obj : x = %f, y = %f, z = %f", rect.origin.x, rect.origin.y, -0.5);
// NSLog(#"N : x = %f, y = %f, z = %f", n[0], n[1], n[2]);
//I'm a viewing volume symmetric projection matrix
// GLfloat P[] = {
// near / right, 0, 0, 0,
// 0, near / top, 0, 0,
// 0, 0, -(far + near) / (far - near), (-2 * far * near) / (far - near),
// 0, 0, -1, 0
// };
GLfloat P[16];
glGetFloatv(GL_PROJECTION_MATRIX, P);
// [self dumpMatrix:P :#"P"];
GLfloat Pminus1[] = {
1/P[0], 0, 0, 0,
0, 1/P[5], 0, 0,
0, 0, 0, 1/P[11],
0, 0, 1/P[14], -(P[10]/ (P[11]*P[14]))
};
// [self dumpMatrix:Pminus1 :#"P-1"];
//clip -> view
GLfloat v[] = {
(Pminus1[0] * n[0]) + (Pminus1[1] * n[1]) + (Pminus1[2] * n[2]) + (Pminus1[3] * n[3]),
(Pminus1[4] * n[0]) + (Pminus1[5] * n[1]) + (Pminus1[6] * n[2]) + (Pminus1[7] * n[3]),
(Pminus1[8] * n[0]) + (Pminus1[9] * n[1]) + (Pminus1[10] * n[2]) + (Pminus1[11] * n[3]),
(Pminus1[12] * n[0]) + (Pminus1[13] * n[1]) + (Pminus1[14] * n[2]) + (Pminus1[15] * n[3])
};
// NSLog(#"v = [%f, %f, %f, %f]", v[0], v[1], v[2], v[3]);
// [self dumpMatrix:mv :#"mv"];
//view -> world
GLfloat Rt[] = {
mv[0], mv[4], -mv[8],
mv[1], mv[5], -mv[9],
-mv[2], -mv[6], mv[10]
};
// NSLog(#"Rt0 = [%f, %f, %f]", Rt[0], Rt[1], Rt[2]);
// NSLog(#"Rt1 = [%f, %f, %f]", Rt[3], Rt[4], Rt[5]);
// NSLog(#"Rt2 = [%f, %f, %f]", Rt[6], Rt[7], Rt[8]);
GLfloat tPrime[] = {
Rt[0] * mv[12] + Rt[1] * mv[13] + Rt[2] * mv[14],
Rt[3] * mv[12] + Rt[4] * mv[13] + Rt[5] * mv[14],
Rt[6] * mv[12] + Rt[7] * mv[13] + Rt[8] * mv[14]
};
// NSLog(#"tPrime = [%f, %f, %f]", tPrime[0], tPrime[1], tPrime[2]);
GLfloat Mminus1[] = {
Rt[0], Rt[1], Rt[2], -(tPrime[0]),
Rt[3], Rt[4], Rt[5], -(tPrime[1]),
Rt[6], Rt[7], Rt[8], -(tPrime[2]),
0, 0, 0, 1
};
//point in world space
GLfloat w[] = {
Mminus1[0] * v[0] + Mminus1[1] * v[1] + Mminus1[2] * v[2] + Mminus1[3] * v[3],
Mminus1[4] * v[0] + Mminus1[5] * v[1] + Mminus1[6] * v[2] + Mminus1[7] * v[3],
Mminus1[8] * v[0] + Mminus1[9] * v[1] + Mminus1[10] * v[2] + Mminus1[11] * v[3],
Mminus1[12] * v[0] + Mminus1[13] * v[1] + Mminus1[14] * v[2] + Mminus1[15] * v[3]
};
NSLog(#"W : x = %f, y = %f, z = %f", w[0], w[1], w[2]);
worldPoint[0] = w[0];
worldPoint[1] = w[1];
worldPoint[2] = w[2];
}
Okay, okay that was still a bit buggy. Here is what is MOSTLY working now:
-(void)view2WorldPoint:(CGPoint)point :(GLfloat*)worldPoint {
float clickX = point.x;
float clickY = point.y;
float clickZ = -near;
//viewport -> normalized device coord -> clip
GLint viewport[4];
glGetIntegerv(GL_VIEWPORT, viewport);
GLfloat n[] = {
(clickX - (float)viewport[0]) / (float)viewport[2] * 2.0 - 1.0,
-((clickY - (float)viewport[1]) / (float)viewport[3] * 2.0 - 1.0),
2.0 * clickZ - 1.0,
1.0
};
GLfloat MP[16], MPInv[16];
MatMatMultiply(MP, projMat, modelMat);
GenerateInverseMatrix4f(MPInv, MP); // replace this one with the whole 1/p thang?
GLfloat w[] = {
(MPInv[0] * n[0]) + (MPInv[4] * n[1]) + (MPInv[8] * n[2]) + (MPInv[12] * n[3]),
(MPInv[1] * n[0]) + (MPInv[5] * n[1]) + (MPInv[9] * n[2]) + (MPInv[13] * n[3]),
(MPInv[2] * n[0]) + (MPInv[6] * n[1]) + (MPInv[10] * n[2]) + (MPInv[14] * n[3]),
(MPInv[3] * n[0]) + (MPInv[7] * n[1]) + (MPInv[11] * n[2]) + (MPInv[15] * n[3])
};
worldPoint[0] = w[0] / w[3];
worldPoint[1] = w[1] / w[3];
worldPoint[2] = w[2] / w[3];
}
float Determinant4f(const float m[16])
{
return
m[12]*m[9]*m[6]*m[3]-
m[8]*m[13]*m[6]*m[3]-
m[12]*m[5]*m[10]*m[3]+
m[4]*m[13]*m[10]*m[3]+
m[8]*m[5]*m[14]*m[3]-
m[4]*m[9]*m[14]*m[3]-
m[12]*m[9]*m[2]*m[7]+
m[8]*m[13]*m[2]*m[7]+
m[12]*m[1]*m[10]*m[7]-
m[0]*m[13]*m[10]*m[7]-
m[8]*m[1]*m[14]*m[7]+
m[0]*m[9]*m[14]*m[7]+
m[12]*m[5]*m[2]*m[11]-
m[4]*m[13]*m[2]*m[11]-
m[12]*m[1]*m[6]*m[11]+
m[0]*m[13]*m[6]*m[11]+
m[4]*m[1]*m[14]*m[11]-
m[0]*m[5]*m[14]*m[11]-
m[8]*m[5]*m[2]*m[15]+
m[4]*m[9]*m[2]*m[15]+
m[8]*m[1]*m[6]*m[15]-
m[0]*m[9]*m[6]*m[15]-
m[4]*m[1]*m[10]*m[15]+
m[0]*m[5]*m[10]*m[15];
}
BOOL GenerateInverseMatrix4f(float i[16], const float m[16])
{
float x=Determinant4f(m);
if (x==0) return FALSE;
i[0]= (-m[13]*m[10]*m[7] +m[9]*m[14]*m[7] +m[13]*m[6]*m[11]
-m[5]*m[14]*m[11] -m[9]*m[6]*m[15] +m[5]*m[10]*m[15])/x;
i[4]= ( m[12]*m[10]*m[7] -m[8]*m[14]*m[7] -m[12]*m[6]*m[11]
+m[4]*m[14]*m[11] +m[8]*m[6]*m[15] -m[4]*m[10]*m[15])/x;
i[8]= (-m[12]*m[9]* m[7] +m[8]*m[13]*m[7] +m[12]*m[5]*m[11]
-m[4]*m[13]*m[11] -m[8]*m[5]*m[15] +m[4]*m[9]* m[15])/x;
i[12]=( m[12]*m[9]* m[6] -m[8]*m[13]*m[6] -m[12]*m[5]*m[10]
+m[4]*m[13]*m[10] +m[8]*m[5]*m[14] -m[4]*m[9]* m[14])/x;
i[1]= ( m[13]*m[10]*m[3] -m[9]*m[14]*m[3] -m[13]*m[2]*m[11]
+m[1]*m[14]*m[11] +m[9]*m[2]*m[15] -m[1]*m[10]*m[15])/x;
i[5]= (-m[12]*m[10]*m[3] +m[8]*m[14]*m[3] +m[12]*m[2]*m[11]
-m[0]*m[14]*m[11] -m[8]*m[2]*m[15] +m[0]*m[10]*m[15])/x;
i[9]= ( m[12]*m[9]* m[3] -m[8]*m[13]*m[3] -m[12]*m[1]*m[11]
+m[0]*m[13]*m[11] +m[8]*m[1]*m[15] -m[0]*m[9]* m[15])/x;
i[13]=(-m[12]*m[9]* m[2] +m[8]*m[13]*m[2] +m[12]*m[1]*m[10]
-m[0]*m[13]*m[10] -m[8]*m[1]*m[14] +m[0]*m[9]* m[14])/x;
i[2]= (-m[13]*m[6]* m[3] +m[5]*m[14]*m[3] +m[13]*m[2]*m[7]
-m[1]*m[14]*m[7] -m[5]*m[2]*m[15] +m[1]*m[6]* m[15])/x;
i[6]= ( m[12]*m[6]* m[3] -m[4]*m[14]*m[3] -m[12]*m[2]*m[7]
+m[0]*m[14]*m[7] +m[4]*m[2]*m[15] -m[0]*m[6]* m[15])/x;
i[10]=(-m[12]*m[5]* m[3] +m[4]*m[13]*m[3] +m[12]*m[1]*m[7]
-m[0]*m[13]*m[7] -m[4]*m[1]*m[15] +m[0]*m[5]* m[15])/x;
i[14]=( m[12]*m[5]* m[2] -m[4]*m[13]*m[2] -m[12]*m[1]*m[6]
+m[0]*m[13]*m[6] +m[4]*m[1]*m[14] -m[0]*m[5]* m[14])/x;
i[3]= ( m[9]* m[6]* m[3] -m[5]*m[10]*m[3] -m[9]* m[2]*m[7]
+m[1]*m[10]*m[7] +m[5]*m[2]*m[11] -m[1]*m[6]* m[11])/x;
i[7]= (-m[8]* m[6]* m[3] +m[4]*m[10]*m[3] +m[8]* m[2]*m[7]
-m[0]*m[10]*m[7] -m[4]*m[2]*m[11] +m[0]*m[6]* m[11])/x;
i[11]=( m[8]* m[5]* m[3] -m[4]*m[9]* m[3] -m[8]* m[1]*m[7]
+m[0]*m[9]* m[7] +m[4]*m[1]*m[11] -m[0]*m[5]* m[11])/x;
i[15]=(-m[8]* m[5]* m[2] +m[4]*m[9]* m[2] +m[8]* m[1]*m[6]
-m[0]*m[9]* m[6] -m[4]*m[1]*m[10] +m[0]*m[5]* m[10])/x;
return TRUE;
}
void MatMatMultiply(GLfloat *result, GLfloat *matrix1, GLfloat *matrix2)
{
result[0]=matrix1[0]*matrix2[0]+
matrix1[4]*matrix2[1]+
matrix1[8]*matrix2[2]+
matrix1[12]*matrix2[3];
result[4]=matrix1[0]*matrix2[4]+
matrix1[4]*matrix2[5]+
matrix1[8]*matrix2[6]+
matrix1[12]*matrix2[7];
result[8]=matrix1[0]*matrix2[8]+
matrix1[4]*matrix2[9]+
matrix1[8]*matrix2[10]+
matrix1[12]*matrix2[11];
result[12]=matrix1[0]*matrix2[12]+
matrix1[4]*matrix2[13]+
matrix1[8]*matrix2[14]+
matrix1[12]*matrix2[15];
result[1]=matrix1[1]*matrix2[0]+
matrix1[5]*matrix2[1]+
matrix1[9]*matrix2[2]+
matrix1[13]*matrix2[3];
result[5]=matrix1[1]*matrix2[4]+
matrix1[5]*matrix2[5]+
matrix1[9]*matrix2[6]+
matrix1[13]*matrix2[7];
result[9]=matrix1[1]*matrix2[8]+
matrix1[5]*matrix2[9]+
matrix1[9]*matrix2[10]+
matrix1[13]*matrix2[11];
result[13]=matrix1[1]*matrix2[12]+
matrix1[5]*matrix2[13]+
matrix1[9]*matrix2[14]+
matrix1[13]*matrix2[15];
result[2]=matrix1[2]*matrix2[0]+
matrix1[6]*matrix2[1]+
matrix1[10]*matrix2[2]+
matrix1[14]*matrix2[3];
result[6]=matrix1[2]*matrix2[4]+
matrix1[6]*matrix2[5]+
matrix1[10]*matrix2[6]+
matrix1[14]*matrix2[7];
result[10]=matrix1[2]*matrix2[8]+
matrix1[6]*matrix2[9]+
matrix1[10]*matrix2[10]+
matrix1[14]*matrix2[11];
result[14]=matrix1[2]*matrix2[12]+
matrix1[6]*matrix2[13]+
matrix1[10]*matrix2[14]+
matrix1[14]*matrix2[15];
result[3]=matrix1[3]*matrix2[0]+
matrix1[7]*matrix2[1]+
matrix1[11]*matrix2[2]+
matrix1[15]*matrix2[3];
result[7]=matrix1[3]*matrix2[4]+
matrix1[7]*matrix2[5]+
matrix1[11]*matrix2[6]+
matrix1[15]*matrix2[7];
result[11]=matrix1[3]*matrix2[8]+
matrix1[7]*matrix2[9]+
matrix1[11]*matrix2[10]+
matrix1[15]*matrix2[11];
result[15]=matrix1[3]*matrix2[12]+
matrix1[7]*matrix2[13]+
matrix1[11]*matrix2[14]+
matrix1[15]*matrix2[15];
}