// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
REAL theta = atan2(v.z,v.x);
REAL psi=-atan2(v.y,pow(v.x*v.x+v.z*v.z,0.5));
Matrix4x4 m1=getRotYMat(theta);
Matrix4x4 m1i=getRotYMat(-theta);
Matrix4x4 m2=getRotZMat(psi);
Matrix4x4 m2i=getRotZMat(-psi);
Matrix4x4 m3=getRotXMat(a);
Matrix4x4 m0=getTransMat(-p);
Matrix4x4 m0i=getTransMat(p);
Matrix4x4 m=m0i*m1i*m2i*m3*m2*m1*m0;
// @TODO: (CAMTRANS) Fill this in...
return m;
}
Matrix4x4 DataProcessor::calcTransMat(std::vector<CS123SceneTransformation*> tranVect){
Matrix4x4 totalMat = Matrix4x4::identity();
//how to handle one transformation
std::vector<CS123SceneTransformation*>::const_iterator i;
for(i = tranVect.begin(); i!=tranVect.end(); i++){
Matrix4x4 tmat;
CS123SceneTransformation cTran = **i;
switch(cTran.type){
case TRANSFORMATION_TRANSLATE:
tmat = getTransMat(cTran.translate);
break;
case TRANSFORMATION_SCALE:
tmat = getScaleMat(cTran.scale);
break;
case TRANSFORMATION_ROTATE:
tmat = getRotMat(Vector4(0,0,0,0),cTran.rotate,cTran.angle);
break;
case TRANSFORMATION_MATRIX:
tmat = cTran.matrix;
break;
}
totalMat = totalMat*tmat;
}
return totalMat;
}
// @returns the rotation matrix around the vector and point by the specified angle
//the angle is in degrees
Matrix4x4 getRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
/* cout<<"point in: "<<p.x<<" "<<p.y<<" "<<p.z<<" "<<p.w<<endl;
cout<<"vec in:"<<v.x<<" "<<v.y<<" "<<v.z<<" "<<v.w<<endl;
cout<<"angle in:"<<a<<endl;*/
Matrix4x4 trans = getTransMat(p);
Matrix4x4 invtrans = getInvTransMat(p);
REAL theta = atan2(v.z,v.x);
REAL phi = -1 * atan2(v.y , sqrt(v.x*v.x + v.z*v.z));
Matrix4x4 M1 = getRotYMat(theta);
Matrix4x4 M2 = getRotZMat(phi);
Matrix4x4 M2Inv = getInvRotZMat(phi);
Matrix4x4 M1Inv = getInvRotYMat(theta);
//Matrix4x4 M3 = getRotXMat(M_PI*a/180);
Matrix4x4 M3 = getRotXMat(a);
Matrix4x4 toReturn = trans * M1Inv * M2Inv * M3 * M2 * M1 * invtrans;
return toReturn;
//return trans * M1 * M2 * M3 * M2Inv * M1Inv * invtrans;
}
void CamtransCamera::translate(const Vector4 &v)
{
// @TODO: [CAMTRANS] Fill this in...
m_eyePosition = getTransMat( v )*m_eyePosition;
updateModelViewMatrix();
}
dhQuat ddARToolkitPlus::getOrientationQuaternion(int markerIndex) {
ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);
getTransMat( &marker, c, m34 );
dhMat matrix;
matrix.v[0] = m34[0][0];
matrix.v[1] = m34[1][0];
matrix.v[2] = m34[2][0];
matrix.v[3] = 0;
matrix.v[4] = m34[0][1];
matrix.v[5] = m34[1][1];
matrix.v[6] = m34[2][1];
matrix.v[7] = 0;
matrix.v[8] = m34[0][2];
matrix.v[9] = m34[1][2];
matrix.v[10] = m34[2][2];
matrix.v[11] = 0;
matrix.v[12] = 0;
matrix.v[13] = 0;
matrix.v[14] = 0;
matrix.v[15] = 1;
/*
dhMat matrix(m34[0][0], m34[0][1], m34[0][2], 0,
m34[1][0], m34[1][1], m34[1][2], 0,
m34[2][0], m34[2][1], m34[2][2], 0,
0, 0, 0, 1);
*/
return matrix.getRotate();
}
// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// [CAMTRANS] Fill this in...
REAL theta = atan2(v.z,v.x);
REAL lambda = a;
REAL phi = -atan2(v.y, sqrt(v.x*v.x+v.z*v.z));
Matrix4x4 To = getInvTransMat(p);// translate to origin
Matrix4x4 M1 = getRotYMat(theta);
Matrix4x4 M2 = getRotZMat(phi);
Matrix4x4 M3 = getRotXMat(lambda);
Matrix4x4 M1inv = getInvRotYMat(theta);
Matrix4x4 M2inv = getInvRotZMat(phi);
Matrix4x4 M = M1inv*M2inv*M3*M2*M1;// full rotation matrix
Matrix4x4 ToInv = getTransMat(p);//translate back to point
Matrix4x4 finalmat = ToInv*M*To;//translate to origin, rotate, and then translate back to point
return finalmat;
}
dhMat ddARToolkitPlus::getMatrix(int markerIndex) {
ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);
getTransMat( &marker, c, m34 );
dhMat matrix;
matrix.v[0] = m34[0][0];
matrix.v[1] = m34[1][0];
matrix.v[2] = m34[2][0];
matrix.v[3] = 0;
matrix.v[4] = m34[0][1];
matrix.v[5] = m34[1][1];
matrix.v[6] = m34[2][1];
matrix.v[7] = 0;
matrix.v[8] = m34[0][2];
matrix.v[9] = m34[1][2];
matrix.v[10] = m34[2][2];
matrix.v[11] = 0;
matrix.v[12] = m34[0][3];
matrix.v[13] = m34[1][3];
matrix.v[14] = m34[2][3];
matrix.v[15] = 1;
return matrix;
}
// @returns the inverse translation matrix described by the vector
Matrix4x4 getInvTransMat(const Vector4 &v) {
const Vector4 &v2=Vector4(-v.x,-v.y,-v.z,1);
return getTransMat(v2);
// @TODO: (CAMTRANS) Fill this in...
}
void ddARToolkitPlus::applyModelMatrix(int markerIndex) {
ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);
getTransMat( &marker, c, m34 );
// Convert from ARTK matrix to OpenGL format
m[0] = m34[0][0];
m[1] = m34[1][0];
m[2] = m34[2][0];
m[3] = 0;
m[4] = m34[0][1];
m[5] = m34[1][1];
m[6] = m34[2][1];
m[7] = 0;
m[8] = m34[0][2];
m[9] = m34[1][2];
m[10] = m34[2][2];
m[11] = 0;
m[12] = m34[0][3];
m[13] = m34[1][3];
m[14] = m34[2][3];
m[15] = 1;
glMatrixMode( GL_MODELVIEW );
glLoadMatrixf( m );
}
dhVector ddARToolkitPlus::getTranslation(int markerIndex) {
ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);
getTransMat( &marker, c, m34 );
dhVector trans(m34[0][3], m34[1][3], m34[2][3]);
return trans;
}
// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// @TODO: [CAMTRANS] Fill this in...
const REAL y = atan2(v.z,v.x), z = -atan2(v.y,std::sqrt(SQ(v.x)+SQ(v.z)));
const Matrix4x4 M1 = getRotYMat(y), M1_inv = getInvRotYMat(y),
M2 = getRotZMat(z), M2_inv = getInvRotZMat(z),
M3 = getRotXMat(a);
return getInvTransMat(-p)*M1_inv*M2_inv*M3*M2*M1*getTransMat(-p);
}
ofQuaternion ofxARToolkitPlus::getOrientationQuaternion(int markerIndex) {
ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);
getTransMat( &marker, c, m34 );
ofMatrix4x4 matrix(m34[0][0], m34[0][1], m34[0][2], 0,
m34[1][0], m34[1][1], m34[1][2], 0,
m34[2][0], m34[2][1], m34[2][2], 0,
0, 0, 0, 1);
return matrix.getRotate();
}
ofMatrix4x4 ofxARToolkitPlus::getMatrix(int markerIndex) {
ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);
getTransMat( &marker, c, m34 );
ofMatrix4x4 matrix(m34[0][0], m34[0][1], m34[0][2], m34[0][3],
m34[1][0], m34[1][1], m34[1][2], m34[1][3],
m34[2][0], m34[2][1], m34[2][2], m34[2][3],
0, 0, 0, 1);
return matrix;
}
void PlanetMaster::addPlanet(Vector3 cam_pos) {
// TODO perturb triangles
random_vals_t *rv;
Vector4 p=stochastic::position(rv,cam_pos);
Vector4 v=stochastic::velocity(rv)/32.0;
int r=rand();
r=r%101;
double rd=r/100.0f;
rd=rd*M_PI/128.0;
int r2=rand();
r2=r2%101;
double ra=r/100.0f;
ra*=M_PI/2.0;
int orbit=rand();
orbit=(orbit)%101;
double orbitd=orbit/100.0f;
orbitd*=M_PI/16.0;
int s=rand();
s=s%32;
s+=64;
double density=(rand()%1000)/999.0;
density=1.0;
int texture=rand();
texture=texture%13;
texture+=3;
// if (texture==1||texture==2){
// texture=3;
// }
Planet* temp = new Planet();
temp->set_velocity(v);
temp->set_scale(getScaleMat(Vector4(s,s,s,1)));
temp->set_radius(s);
temp->set_trans(getTransMat(p));
temp->set_axis_angle(ra);
temp->set_density(density);
Matrix4x4 rot=getRotXMat(rd);
temp->set_rot(rot);
temp->set_orbit_rot(getRotZMat(orbitd));
temp->calculate_composite_transformations();
temp->set_texture(texture);
m_planets.append(temp);
}
void testMatrix::testMatrices()
{
Vector4 testTransV = Vector4(4,5,6,1);
Matrix4x4 testTrans = Matrix4x4(1,0,0,-4,0,1,0,-5,0,0,1,-6,0,0,0,1);
Matrix4x4 testRotZ90 = Matrix4x4(0.f,-1.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,1.f);
Matrix4x4 testRotY90 = Matrix4x4(0.f,0.f,1.f,0.f,0.f,1.f,0.f,0.f,-1.f,0.f,0.f,0.f,0.f,0.f,0.f,1.f);
Matrix4x4 testRotX90 = Matrix4x4(1.f,0.f,0.f,0.f,0.f,0.f,-1.f,0.f,0.f,1.f,0.f,0.f,0.f,0.f,0.f,1.f);
//y
Matrix4x4 testRot1 = getRotMat(Vector4(0,0,0,1),Vector4(0,1,0,1),90);
//x
Matrix4x4 testRot2 = getRotMat(Vector4(0,0,0,1),Vector4(1,0,0,1), 90);
//z
Matrix4x4 testRot3 = getRotMat(Vector4(0,0,0,1),Vector4(0,0,1,1), 90);
Matrix4x4 trz90 = getRotZMat(M_PI/2);
Matrix4x4 try90 = getRotYMat(M_PI/2);
Matrix4x4 trx90 = getRotXMat(M_PI/2);
Matrix4x4 trans = getTransMat(testTransV);
//Test rotation on z axis
compareMatrices(&testRotZ90,&trz90);
//Z axis as arbitrary rotation
compareMatrices(&testRotZ90,&testRot3);
//inverses are equivalent:
compareMatrices(&getInvRotZMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(0,0,1,1),90));
//Test rotation on y axis
compareMatrices(&testRotY90,&try90);
//Y axis as arbitrary rotation
compareMatrices(&testRotY90,&testRot1);
//inverses are equivalent:
compareMatrices(&getInvRotYMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(0,1,0,1),90));
//Test rotation on x axis
compareMatrices(&testRotX90,&trx90);
//X axis as arbitrary rotation
compareMatrices(&testRotX90,&testRot2);
//inverses are equivalent:
compareMatrices(&getInvRotXMat(M_PI/2),&getInvRotMat(Vector4(0,0,0,1),Vector4(1,0,0,1),90));
//test translation
compareMatrices(&testTrans,&trans);
}
void ofxARToolkitPlus::getTranslationAndOrientation(int markerIndex, ofVec3f &translation, ofMatrix4x4 &orientation) {
ARToolKitPlus::ARMarkerInfo marker = tracker->getDetectedMarker(markerIndex);
getTransMat( &marker, c, m34 );
// Translation
translation.set(m34[0][3], m34[1][3], m34[2][3]);
// Orientation
orientation.set(m34[0][0], m34[0][1], m34[0][2], 0,
m34[1][0], m34[1][1], m34[1][2], 0,
m34[2][0], m34[2][1], m34[2][2], 0,
0, 0, 0, 1);
}
// @returns the inverse rotation matrix around the vector and point by the specified angle
Matrix4x4 getInvRotMat (const Point3 &p, const Vector3 &v, const real_t a) {
const real_t vZ = v[2];
const real_t vX = v[0];
const real_t theta = atan2(vZ, vX);
const real_t phi = -atan2(v[1], sqrt(vX * vX + vZ * vZ));
const Matrix4x4 &transToOrigin = getInvTransMat(Vector3(p[0], p[1], p[2]));
const Matrix4x4 &A = getRotYMat(theta);
const Matrix4x4 &B = getRotZMat(phi);
const Matrix4x4 &C = getRotXMat(a);
const Matrix4x4 &invA = getInvRotYMat(theta);
const Matrix4x4 &invB = getInvRotZMat(phi);
const Matrix4x4 &transBack = getTransMat(Vector3(p[0], p[1], p[2]));
return transBack * (invA * invB * C * B * A).getTranspose() * transToOrigin;
}
// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// angles to rotate v to x-axis
REAL th = atan2(v.z,v.x),
ph = -atan2(v.y,sqrt(pow(v.z,2)+pow(v.x,2)));
// all transformations required
// Note: Inverse of Rot mat is just transpose
Matrix4x4 T = getInvTransMat(p),
Tinv = getTransMat(p),
M1 = getRotYMat(th),
M2 = getRotZMat(ph),
M3 = getRotXMat(a);
return Tinv * M1.getTranspose() * M2.getTranspose()
* M3 * M2 * M1 * T;
}
// @returns the rotation matrix around the vector and point by the specified angle
Matrix4x4 getRotMat (const Vector4 &p, const Vector4 &v, const REAL a) {
// [PASS]
REAL theta = atan2(v.z, v.x);
REAL phi = -atan2(v.y, sqrt(v.x*v.x + v.z*v.z));
// translate to the origin and back
Matrix4x4 Mt = getTransMat(-p);
Matrix4x4 Mt_1 = getInvTransMat(-p);
Matrix4x4 M1 = getRotYMat(theta);
Matrix4x4 M2 = getRotZMat(phi);
Matrix4x4 M3 = getRotXMat(a);
Matrix4x4 M1_1 = getInvRotYMat(theta);
Matrix4x4 M2_1 = getInvRotZMat(phi);
return Mt_1*M1_1*M2_1*M3*M2*M1*Mt;
}
// @returns the inverse rotation matrix around the vector and
// point by the specified angle
Matrix4x4 getInvRotMat (const Vector4 &h, const Vector4 &a, const REAL lambda) {
// @DONE: [CAMTRANS] Filled in.
double theta = atan2(a.z, a.x);
double phi = -atan2(a.y, sqrt(a.x*a.x + a.z*a.z));
//want to translate by -h to move from h to the origin
Matrix4x4 Th = getTransMat(-h);
Matrix4x4 M1Inv = getInvRotYMat(theta);
Matrix4x4 M2Inv = getInvRotZMat(phi);
Matrix4x4 M3Inv = getInvRotXMat(lambda);
Matrix4x4 M2 = getRotZMat(phi);
Matrix4x4 M1 = getRotYMat(theta);
Matrix4x4 ThInv = getInvTransMat(-h);
Matrix4x4 result = ThInv * M1Inv * M2Inv * M3Inv * M2 * M1 * Th;
return result;
}
////////////////////////////////////////////////////////////////////////////////
// GET VIEW MATRIX
////////////////////////////////////////////////////////////////////////////////
glm::mat4 Camera::getView() const
{
return glm::inverse(getTransMat());
}
// @returns the inverse translation matrix described by the vector
Matrix4x4 getInvTransMat(const Vector4 &v) {
return getTransMat(v*-1);
}
void LappedUtils::assignSeedUV(PatchTri* seed, vec2<float> &v0st, vec2<float> &v1st, vec2<float> &v2st)
{
float scale = .25;
Vector4 A,B,C;
A = seed->v0->pos;
B = seed->v1->pos;
C = seed->v2->pos;
A.w = 1;
B.w = 1;
C.w = 1;
Vector4 ctr = (A+B+C)/3.0;
ctr.w=0;
Vector4 Ap, Bp, Cp, Tp;
Matrix4x4 transMat = getTransMat(-ctr);
Ap = transMat*A;
Bp = transMat*B;
Cp = transMat*C;
Ap.w=0;
Bp.w=0;
Cp.w=0;
Vector4 norm = ((Bp-Ap).cross(Cp-Ap)).getNormalized();
printVector4(norm);
double angle = acos(norm.dot(Vector4(0,0,1.0,0)));
//cout << "angle between normal and plane z=1: " << angle << endl;
if (norm.cross(Vector4(0,0,1,0)).getMagnitude()>0.00001)
{
//cout << (norm.cross(Vector4(0,0,1,0))).getMagnitude() << endl;
//cout << norm.cross(Vector4(0,0,1,0)) << endl;
Matrix4x4 rotMat = getRotMat(Vector4(0,0,0,0),norm.cross(Vector4(0,0,1.0,0)).getNormalized(),angle);
Ap.w = 1;
Bp.w = 1;
Cp.w = 1;
Ap = rotMat*Ap;
Bp = rotMat*Bp;
Cp = rotMat*Cp;
Tp = rotMat*seed->tangent;
}
else Tp=seed->tangent;
Ap.w = 0;
Bp.w = 0;
Cp.w = 0;
double avgLen = max(max((Ap-Cp).getMagnitude(),(Ap-Bp).getMagnitude()),(Cp-Bp).getMagnitude());
Ap = Ap*scale/avgLen;
Bp = Bp*scale/avgLen;
Cp = Cp*scale/avgLen;
Ap.w = 1;
Bp.w = 1;
Cp.w = 1;
Tp.w = 0;
seed->tangent = Tp;
Tp.normalize();
double tanAngle = acos(Vector4(0,1,0,0).dot(Tp));
Matrix4x4 tanMat = getRotMat(Vector4(0,0,0,0),Vector4(0,1,0,0).cross(Tp),tanAngle);
Ap = tanMat*Ap;
Bp = tanMat*Bp;
Cp = tanMat*Cp;
Ap.w = 1;
Bp.w = 1;
Cp.w = 1;
transMat = getTransMat(Vector4(.5,.5,0,0));
Ap = transMat*Ap;
Bp = transMat*Bp;
Cp = transMat*Cp;
v0st.x = Ap.x;
v0st.y = Ap.y;
v1st.x = Bp.x;
v1st.y = Bp.y;
v2st.x = Cp.x;
v2st.y = Cp.y;
}
void PlanetMaster::create_solar_system(){
Vector4 v,p;
double s,ra, rd, mass,texture;
Matrix4x4 rot;
//make a sun
Planet* sun=new Planet();
s=400;
ra=0.0;
rd=0.01;
v=Vector4(0,0,0,0);
texture=15;
p=Vector4(0.0,0.0,0.0,1.0);
mass=100000000.0;
Planet * temp=sun;
temp->set_velocity(v);
temp->set_scale(getScaleMat(Vector4(s,s,s,1)));
temp->set_radius(s);
temp->set_trans(getTransMat(p));
temp->set_axis_angle(ra);
temp->set_mass(mass);
rot=getRotXMat(rd);
temp->set_rot(rot);
temp->calculate_composite_transformations();
temp->set_texture(texture);
m_planets.append(temp);
Planet* earth=new Planet();
mass=1.0;
temp=earth;
s=32;
texture=10;
p=Vector4(512.0,0.0,0.0,1.0);
v=Vector4(0.0,0.0,50.0,0.0);
temp->set_velocity(v);
temp->set_scale(getScaleMat(Vector4(s,s,s,1)));
temp->set_radius(s);
temp->set_trans(getTransMat(p));
temp->set_axis_angle(ra);
temp->set_mass(mass);
rot=getRotXMat(rd);
temp->set_rot(rot);
temp->calculate_composite_transformations();
temp->set_texture(texture);
m_planets.append(temp);
Planet* jupiter=new Planet();
mass=317.8;
temp=jupiter;
s=96;
texture=14;
p=Vector4(-1024.0,0.0,0.0,1.0);
v=Vector4(0.0,0.0,-30.0,0.0);
temp->set_velocity(v);
temp->set_scale(getScaleMat(Vector4(s,s,s,1)));
temp->set_radius(s);
temp->set_trans(getTransMat(p));
temp->set_axis_angle(ra);
temp->set_mass(mass);
rot=getRotXMat(rd);
temp->set_rot(rot);
temp->calculate_composite_transformations();
temp->set_texture(texture);
m_planets.append(temp);
}
