Mat4 Mat4::Transpose() const
{
return Mat4(
m[ 0], m[ 1], m[ 2], m[ 3],
m[ 4], m[ 5], m[ 6], m[ 7],
m[ 8], m[ 9], m[10], m[11],
m[12], m[13], m[14], m[15] );
}
//---------------------------------------------------------------------
Mat4 DXMapping::fromDXMatrix( const D3DXMATRIX& mat )
{
return Mat4(
mat.m[0][0], mat.m[1][0], mat.m[2][0], mat.m[3][0],
mat.m[0][1], mat.m[1][1], mat.m[2][1], mat.m[3][1],
mat.m[0][2], mat.m[1][2], mat.m[2][2], mat.m[3][2],
mat.m[0][3], mat.m[1][3], mat.m[2][3], mat.m[3][3]);
}
Mat4 I() {
float a[] = {
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
};
return Mat4(a);
}
Mat4 frustum(float l, float r, float t, float b, float n, float f) {
float a[] = {
dnnLbd(n, r, l), 0.0f, nLbd(r, l), 0.0f,
0.0f, dnnLbd(n, t, b), nLbd(t, b), 0.0f,
0.0f, 0.0f, -nLbd(f, n), dnnLbd(n*f, f, n),
0.0f, 0.0f, -1.0f, 0.0f
};
return Mat4(a);
}
/* Creates a 4-by-4 matrix which rotates around the z-axis by the given angle.*/
Mat4 rotateZ(double theta) {
double c = cos(theta);
double s = sin(theta);
return Mat4(
c, s, 0.0, 0.0,
-s, c, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0);
};
/* Computes a perspective transformation matrix given the angular height
of the frustum, the aspect ratio, and the near and far clipping planes.*/
Mat4 perspective(double angle, double aspect, double near, double far)
{
double f = tan(0.5 * (M_PI - angle));
double range = near - far;
return Mat4(f / aspect, 0.0, 0.0, 0.0,
0.0, f, 0.0, 0.0,
0.0, 0.0, (far + near) / range, -1.0,
0.0, 0.0, 2.0 * near * far / range, 0.0);
};
/* Computes an orthographic projection matrix given the coordinates of the
planes defining the viewing volume.*/
Mat4 orthographic(double left, double right, double bottom, double top, double near, double far) {
return Mat4(
2.0 / (right - left), 0.0, 0.0, 0.0,
0.0, 2.0 / (top - bottom), 0.0, 0.0,
0.0, 0.0, 2.0 / (far - near), 0.0,
(left + right) / (left - right),
(bottom + top) / (bottom - top),
(near + far) / (far - near), 1.0);
};
Mat AlignShape (SHAPE &Shape, // io
const SHAPE &AnchorShape, // in
const Vec *pWeights) // in: can be NULL
{
CheckSameNbrRows(Shape, AnchorShape, "AlignShape");
if (pWeights && pWeights->nrows() == 0)
pWeights = NULL;
if (pWeights)
ASSERT(Shape.nrows() == pWeights->nelems());
double X1 = 0, Y1 = 0, X2 = 0, Y2 = 0, W = 0, Z = 0, C1 = 0, C2 = 0;
int iRow = Shape.nrows();
while (iRow--)
{
const double x1 = AnchorShape(iRow, VX);
const double y1 = AnchorShape(iRow, VY);
const double x2 = Shape(iRow, VX);
const double y2 = Shape(iRow, VY);
if (x1 == 0 && y1 == 0) // is anchor landmark unused?
;
else if (x2 == 0 && y2 == 0) // is landmark unused?
;
else
{
const double w = (pWeights? (*pWeights)(iRow): 1.0);
W += w;
Z += w * (x2 * x2 + y2 * y2);
X1 += w * x1;
Y1 += w * y1;
X2 += w * x2;
Y2 += w * y2;
C1 += w * (x1 * x2 + y1 * y2);
C2 += w * (y1 * x2 - x1 * y2);
}
}
double SolnData[] = { X2, -Y2, W, 0,
Y2, X2, 0, W,
Z, 0, X2, Y2,
0, Z, -Y2, X2 };
MatView Mat4(SolnData, 4, 4, 0); // 4x4, tda=0
double VecData[] = { X1, Y1, C1, C2 };
VecView Vec4(VecData, 4);
Vec Soln(SolveWithLU(Mat4, Vec4));
double TransformData[] = { Soln(0), -Soln(1), Soln(2), // a b tx
Soln(1), Soln(0), Soln(3), // c d ty
0, 0, 1 };
Mat Transform(TransformData, 3, 3);
Shape = TransformShape(Shape, Transform);
return Transform;
}
/* Computes a perspective transformation matrix given the left, right,
top, bottom, near and far clipping planes.*/
Mat4 frustum(double left, double right, double bottom, double top, double near, double far) {
double dx = (right - left);
double dy = (top - bottom);
double dz = (near - far);
return Mat4(
2.0 * near / dx, 0.0, 0.0, 0.0,
0.0, 2.0 * near / dy, 0.0, 0.0,
(left + right) / dx, (top + bottom) / dy, far / dz, -1.0,
0.0, 0.0, near * far / dz, 0.0);
};
Mat4 lookAt(const Vec3& eye, const Vec3& center, const Vec3& up)
{
Vec3 c_forward = (center - eye).normalize();
Vec3 c_right = c_forward.cross(up).normalize();
Vec3 c_up = c_right.cross(c_forward).normalize();
return Mat4(c_right.x, c_up.x, -c_forward.x, 0,
c_right.y, c_up.y, -c_forward.y, 0,
c_right.z, c_up.z, -c_forward.z, 0,
0, 0, 0, 1) * translation(-eye);
}
static std::list<Eigen::Matrix<double,4,12> > lqr(System const & sys, double dt, std::list<State> const & list_state, std::list<U> const & list_u, Mat12 Q, Mat4 Ru, Mat12 Qf)
{
std::list<Eigen::Matrix<double,4,12> > list_K;
std::list<State>::const_reverse_iterator rit_state=list_state.crbegin();
std::list<U>::const_reverse_iterator rit_u=list_u.crbegin();
rit_state++;
Mat4 MF=(Mat4()<< 0, sys.kt*sys.d, 0, -sys.kt*sys.d,
-sys.kt*sys.d, 0, sys.kt*sys.d, 0,
sys.km, -sys.km, sys.km, -sys.km,
sys.kt, sys.kt, sys.kt, sys.kt).finished();
Mat12 P=Qf;
while(rit_state!=list_state.crend())
{
Vec4 u1=*rit_u;
Vec4 u2=u1.cwiseProduct(u1);
Mat3 R=rit_state->g.block(0,0,3,3);
Vec3 omega=rit_state->v.head(3);
Mat3 Ad=SO3::exp(-omega*dt);
double f=(u2(0)+u2(1)+u2(2)+u2(3))*sys.kt;
Mat12 A=Mat12::Zero();
A.block(0,6,6,6)=Mat6::Identity();
A.block(6,6,3,3)=sys.I_inv*(SO3::hat(sys.I*omega)-SO3::hat(omega)*sys.I);
A.block(9,0,3,3)=-f*R*SO3::E[2]/sys.m;
A.block(0,6,3,6)=SO3::dexp(omega*dt)*A.block(0,6,3,6);
A=Mat12::Identity()+A*dt;
A.block(0,0,3,12)=SO3::exp(-omega*dt)*A.block(0,0,3,12);
Eigen::Matrix<double,12,4> B=Eigen::Matrix<double,12,4>::Zero();
B.block(6,0,3,3)=sys.I_inv;
B.block(9,3,3,1)=R.col(2)/sys.m;
B=B*MF*(2*u1).asDiagonal();
B=B*dt;
Eigen::Matrix<double,4,12> K=(Ru+B.transpose()*P*B).inverse()*B.transpose()*P*A;
P=K.transpose()*Ru*K+Q+(A-B*K).transpose()*P*(A-B*K);
list_K.push_front(K);
rit_state++;
rit_u++;
}
return list_K;
}
void Camera::setPerspectiveMatrix(GLfloat near, GLfloat far, GLfloat fov) {
_near = near;
_far = far;
_fov = fov;
GLfloat s = 1/tan(_fov*0.5*PI/180);
GLfloat p = -_far/(_far-_near);
GLfloat mat[16] = {s,0,0,0,0,s,0,0,0,0,p,p*_near,0,0,-1,0};
_projection = Mat4(mat);
}
Mat4 translate(const Mat4& m, Vec4& v) {
float x = v.x();
float y = v.y();
float z = v.z();
float a[] = {
0.0f, 0.0f, 0.0f, x,
0.0f, 0.0f, 0.0f, y,
0.0f, 0.0f, 0.0f, z,
0.0f, 0.0f, 0.0f, 0.0f
};
return Mat4(a) + m;
}
Mat4 Mat4::operator-(const Mat4& right) const
{
const float *matrix1 = dataPointer();
const float *matrix2 = right.dataPointer();
float matrix3[16];
for (int i = 0; i < 16; ++i) {
matrix3[i] = matrix1[i] - matrix2[i];
}
return Mat4(matrix3);
}
Mat4 scale(const Mat4& m, Vec4& v) {
float x = v.x();
float y = v.y();
float z = v.z();
float a[] = {
x, 0.0f, 0.0f, 0.0f,
0.0f, y, 0.0f, 0.0f,
0.0f, 0.0f, z, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
};
return Mat4(a);
}
static GeometryPtr createPatch(const float P[12], const float Cs[3],
const Mat4& trans = Mat4())
{
PrimvarStorageBuilder builder;
builder.add(Primvar::P, P, 12);
builder.add(PrimvarSpec(PrimvarSpec::Constant, VarSpec::Color, 1,
ustring("Cs")), Cs, 3);
IclassStorage storReq(1,4,4,4,4);
GeometryPtr patch(new Patch(builder.build(storReq)));
patch->transform(trans);
return patch;
}
Mat4 ortho(float left, float right, float bottom, float top, float zNear, float zFar)
{
float w = right - left;
float h = top - bottom;
float d = zFar - zNear;
float tx = -(right + left) / w;
float ty = -(top + bottom) / h;
float tz = -(zFar + zNear) / d;
return Mat4(2.f / w, 0, 0, 0, 0, 2.f / h, 0, 0, 0, 0, -2.f / d, 0, tx, ty, tz, 1.f);
}
void PrimitiveCube::applyTransform(GLCamera3D* camera)
{
GLuint projLoc;
GLuint worldLoc;
GLuint viewLoc;
GLuint colorLoc;
m_program->GetUniformLoc(SHADER_GLOBAL_PROJECTION, projLoc);
m_program->GetUniformLoc(SHADER_GLOBAL_VIEW, viewLoc);
m_program->GetUniformLoc(SHADER_GLOBAL_WORLD, worldLoc);
m_program->GetUniformLoc("gColor", colorLoc);
if(!m_usingCustomWorld) {
Mat4 world = Mat4(0.0f);
m_world =
glm::translate(Mat4(1.0f), m_position) * //apply position
glm::rotate(Mat4(1.0f), m_rotation.x, Vec3(1, 0, 0)) *
glm::rotate(Mat4(1.0f), m_rotation.y, Vec3(0, 1, 0)) *
glm::rotate(Mat4(1.0f), m_rotation.y, Vec3(0, 0, 1)) *
glm::scale(Mat4(1.0f), m_scale);
}
/*apply value from camera*/
glUniformMatrix4fv(projLoc, 1, GL_FALSE, glm::value_ptr(camera->Projection()));
glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(camera->View()));
glUniformMatrix4fv(worldLoc, 1, GL_FALSE, glm::value_ptr(m_world));
glUniform4f(colorLoc, m_color.r, m_color.g, m_color.b, m_color.a);
}
void renderDofAmountTest()
{
Options opts;
opts.xRes = 320;
opts.yRes = 240;
opts.gridSize = 8;
opts.clipNear = 0.1;
opts.superSamp = Imath::V2i(10,10);
opts.pixelFilter = makeGaussianFilter(Vec2(2.0,2.0));
opts.fstop = 100;
opts.focalLength = 20;
opts.focalDistance = 3;
Attributes attrs;
attrs.shadingRate = 1;
attrs.smoothShading = true;
Mat4 camToScreen =
perspectiveProjection(90, opts.clipNear, opts.clipFar) *
screenWindow(-2.33333, 0.33333, -1, 1);
// Output variables.
VarList outVars;
outVars.push_back(Stdvar::Cs);
Renderer r(opts, camToScreen, outVars);
Mat4 wToO = Mat4().setTranslation(Vec3(-0.5,-0.5,-1)) *
Mat4().setAxisAngle(Vec3(0,0,1), deg2rad(45)) *
Mat4().setTranslation(Vec3(-1.5, 0, 2));
const float P[12] = {0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 1, 0};
{
const float Cs[3] = {1, 0.7, 0.7};
r.add(createPatch(P, Cs, wToO), attrs);
}
{
const float Cs[3] = {0.7, 1, 0.7};
r.add(createPatch(P, Cs, Mat4().setTranslation(Vec3(0,0,1))*wToO), attrs);
}
{
const float Cs[3] = {0.7, 0.7, 1};
r.add(createPatch(P, Cs, Mat4().setTranslation(Vec3(0,0,2))*wToO), attrs);
}
{
const float Cs[3] = {1, 0.7, 0.7};
r.add(createPatch(P, Cs, Mat4().setTranslation(Vec3(0,0,5))*wToO), attrs);
}
{
const float Cs[3] = {0.7, 1, 0.7};
r.add(createPatch(P, Cs, Mat4().setTranslation(Vec3(0,0,25))*wToO), attrs);
}
r.render();
}
Mat4 Mat4::rotateZ(const float &theta)
{
#if defined RADIANS
float cosT = cosf(theta); // theta in radians
float sinT = sinf(theta); // theta in radians
#elif defined DEGREES
float cosT = cosf(theta * M_PI / 180.0f); // theta in degrees
float sinT = sinf(theta * M_PI / 180.0f); // theta in degrees
#endif
#ifdef RIGHTHANDED
return Mat4( cosT, sinT, 0.0f, 0.0f,
-sinT, cosT, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
#elif defined LEFTHANDED
return Mat4( cosT,-sinT, 0.0f, 0.0f,
sinT, cosT, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
#endif
}
//==============================================================================
void Camera::onMoveComponentUpdate(SceneNode&, F32, F32)
{
// Frustum
FrustumComponent& fr = *this;
fr.setViewMatrix(Mat4(getWorldTransform().getInverse()));
fr.setViewProjectionMatrix(fr.getProjectionMatrix() * fr.getViewMatrix());
fr.getFrustum().resetTransform(getWorldTransform());
// Spatial
SpatialComponent& sp = *this;
sp.markForUpdate();
}
//==============================================================================
void CollisionDebugDrawer::visit(const ConvexHullShape& hull)
{
m_dbg->setModelMatrix(Mat4(hull.getTransform()));
m_dbg->begin(GL_LINES);
const Vec4* points = hull.getPoints() + 1;
const Vec4* end = hull.getPoints() + hull.getPointsCount();
for(; points != end; ++points)
{
m_dbg->pushBackVertex(hull.getPoints()->xyz());
m_dbg->pushBackVertex(points->xyz());
}
m_dbg->end();
}
Mat4 perspective(float left, float right, float bottom, float top, float zNear, float zFar)
{
float w = right - left;
float h = top - bottom;
float d = zFar - zNear;
float n2 = zNear * 2.f;
float A = (right + left) / w;
float B = (top + bottom) / h;
float C = -(zFar + zNear) / d;
float D = -n2 * zFar / d;
return Mat4(n2 / w, 0, 0, 0, 0, n2 / h, 0, 0, A, B, C, -1, 0, 0, D, 0);
}
void TrackballCameraController::Track(float x,float y)
{
//
// Mat4 mat(1.0);
// mat = glm::rotate(mat,y*mRotationScaler,mRight);
// mat = glm::rotate(mat,x*mRotationScaler,Vec3(0.0f, glm::dot(mCamera->UpVec(), Vec3(0, 1, 0)) < 0 ? -1.0f : 1.0f, 0.0f));
// Vec3 pos = (mCamera->Eye())*Mat3(mat);
// Vec3 target = mCamera->Tareget()*Mat3(mat);
// Vec3 up = Vec3(0.0f, glm::dot(mCamera->UpVec(), Vec3(0, 1, 0)) < 0 ? -1.0f : 1.0f, 0.0f);
// mRight = glm::normalize(glm::cross(up, -pos));
// mCamera->LookAt(pos, target, mCamera->UpVec());
/*Quat q = MathLib::rotation_axis(mRight, y * mRotationScaler);
Mat4 mat = MathLib::transformation(mTarget, q);
Vec3 pos = MathLib::transform_coord(mCamera->Eye(), mat);
q = MathLib::rotation_axis(Vec3(0.0f, glm::dot(mCamera->UpVec(), Vec3(0, 1, 0)) < 0 ? -1.0f : 1.0f, 0.0f), x * mRotationScaler);
mat = MathLib::transformation(mTarget, q);
pos = MathLib::transform_coord(pos, mat);
mRight = MathLib::transform_quat(mRight, q);
Vec3 dir;
if (mReverseTarget)
{
dir = pos - mTarget;
}
else
{
dir = mTarget - pos;
}
dir = glm::normalize(dir);
Vec3 up = glm::cross(dir, mRight);
mCamera->LookAt(pos, pos + dir, up);*/
Mat4 mat,matx,maty;
mat = matx = maty = Mat4(1.0);
matx = glm::rotate(matx,x*mRotationScaler,Vec3(0,1,0));
maty = glm::rotate(matx,y*mRotationScaler,Vec3(1,0,0));
mat = matx*maty;
Vec3 pos = (mCamera->Eye())*Mat3(mat);
Vec3 target = mCamera->Tareget()*Mat3(mat);
Vec3 up = mCamera->UpVec()*Mat3(mat);
//Vec3 up = Vec3(0.0f, glm::dot(mCamera->UpVec(), Vec3(0, 1, 0)) < 0 ? -1.0f : 1.0f, 0.0f);
//mRight = glm::normalize(glm::cross(up, -pos));
mCamera->LookAt(pos, target,up);
}
void PrimitiveTube::applyTransform(GLCamera3D* camera)
{
GLuint projLoc;
GLuint worldLoc;
GLuint viewLoc;
m_program->GetUniformLoc(SHADER_GLOBAL_PROJECTION, projLoc);
m_program->GetUniformLoc(SHADER_GLOBAL_VIEW, viewLoc);
m_program->GetUniformLoc(SHADER_GLOBAL_WORLD, worldLoc);
Mat4 world = Mat4(0.0f);
world =
glm::translate(Mat4(1.0f), m_position) * //apply position
glm::rotate(Mat4(1.0f), m_rotationX, Vec3(1, 0, 0)) *
glm::rotate(Mat4(1.0f), m_rotationY, Vec3(0, 1, 0)) *
glm::rotate(Mat4(1.0f), m_rotationZ, Vec3(0, 0, 1));
/*apply value from camera*/
glUniformMatrix4fv(projLoc, 1, GL_FALSE, glm::value_ptr(camera->Projection()));
glUniformMatrix4fv(viewLoc, 1, GL_FALSE, glm::value_ptr(camera->View()));
glUniformMatrix4fv(worldLoc, 1, GL_FALSE, glm::value_ptr(world));
}
void TestParticleSystem::addParticle()
{
TestParticle* particle = TestParticle::create();
// Time to live
particle->timeToLive = MAX(life + lifeVar * CCRANDOM_MINUS1_1(), 0.0);
// Position
Vec2 srcPos = Vec2();
particle->pos = Vec2(srcPos.x + posVar.x * CCRANDOM_MINUS1_1(),
srcPos.y + posVar.y * CCRANDOM_MINUS1_1());
// Size
particle->size = MAX(startSize + startSizeVar * CCRANDOM_MINUS1_1(), 0.0);
float endSizeFinal = MAX(endSize + endSizeVar * CCRANDOM_MINUS1_1(), 0.0);
particle->deltaSize = (endSizeFinal - particle->size) / particle->timeToLive;
// Rotation
particle->rotation = startRotation + startRotationVar * CCRANDOM_MINUS1_1();
float endRotationFinal = endRotation + endRotationVar * CCRANDOM_MINUS1_1();
particle->deltaRotation = (endRotationFinal - particle->rotation) / particle->timeToLive;
// Direction
float dirRadians = CC_DEGREES_TO_RADIANS(direction + directionVar * CCRANDOM_MINUS1_1());
Vec2 dirVector = Vec2(cosf(dirRadians), sin(dirRadians));
float speedFinal = speed + speedVar * CCRANDOM_MINUS1_1();
dirVector.scale(speedFinal);
particle->dir = dirVector;
// Accelerations
particle->radialAccel = radialAcceleration + radialAccelerationVar * CCRANDOM_MINUS1_1();
particle->tangentialAccel = tangentialAcceleration + tangentialAccelerationVar;
// Colors
particle->simpleColorSequence[0] = 255.0;
particle->simpleColorSequence[1] = 255.0;
particle->simpleColorSequence[2] = 255.0;
particle->simpleColorSequence[3] = 255.0;
particle->simpleColorSequence[4] = 255.0;
particle->simpleColorSequence[5] = 0.0;
particle->simpleColorSequence[6] = 0.0;
particle->simpleColorSequence[7] = 0.0;
particle->transform = Mat4();
particles.pushBack(particle);
numEmittedParticles++;
}
/* Computes a look-at transformation.*/
Mat4 lookAt(const Vec3& eye, const Vec3& target, const Vec3& up) {
Vec3 vz = eye - target;
Vec3 vx = up.cross(vz);
Vec3 vy = vz.cross(vx);
vx /= vx.mag();
vy /= vy.mag();
vz /= vz.mag();
return Mat4(
vx.x, vx.y, vx.z, 0.0,
vy.x, vy.y, vy.z, 0.0,
vz.x, vz.y, vz.z, 0.0,
eye.x, eye.y, eye.z, 1.0).inverse();
};
Mat4* Camera::matrixLookAtLh(Mat4* pOut, Vec3* pEye, Vec3* pAt, Vec3* pUp)
{
// Transform of the matrix below.
// http://msdn.microsoft.com/en-us/library/windows/desktop/bb205342(v=vs.85).aspx
Vec3 look = (*pAt - *pEye).normal();
Vec3 right = cross(*pUp, look).normal();
Vec3 up = cross(look, right).normal();
*pOut = Mat4(right.x, right.y, right.z, -dot(right, *pEye),
up.x, up.y, up.z, -dot(up, *pEye),
look.x, look.y, look.z, -dot(look, *pEye),
0.0f, 0.0f, 0.0f, 1.0f);
return pOut;
}
VOID RenderTargetScene::SendMatricesToProgram( GLSLProgram* pProg, const Mat4* arrMatrices, PTriMesh pMesh ) const
{
// Get model matrix
Mat4 matModel = Mat4(pMesh->m_matLocal.get());
/*if (pMesh->GetParentMesh())
{
Vec3 vModelPos = pMesh->m_pMatLocal.getPosition();
PTriMesh pParent = pMesh->GetParentMesh();
Vec3 vParent = pParent->m_pMatLocal.getPosition();
matModel.setPosition(vModelPos + vParent);
}*/
// Get view matrix
Mat4 matView = Mat4(arrMatrices[MATRIX_VIEW_ID].get());
// Get Proj matrix
Mat4 matProj = Mat4(arrMatrices[MATRIX_PROJ_ID].get());
pProg->SendMatrices(matModel.get(), matView.get(), matProj.get());
UINT unfID = pProg->GetUniformLocation("depthMVP");
if (unfID != -1)
{
Mat4 matDepthMVP = matModel * arrMatrices[MATRIX_VIEW_LIGHT_ID];
m_pOpenGL->glUniformMatrix4fv(unfID, 1, GL_FALSE, matDepthMVP.get() );
}
// vScreenData
//////////////////////////////////////////////////////////////////////////
unfID = pProg->GetUniformLocation("vScreenData");
if (unfID != -1)
{
float fTime = ENG::GetElapsedTime() * 0.015f;
float vScreenData[4] = {ENG::SCR_WID_INV, ENG::SCR_HEI_INV, fTime, s_STEREO ? 0.0f : 1.0f };
m_pOpenGL->glUniform4fv(unfID, 1, vScreenData);
}
}
// getRotationMatrix taken from Android's SensorManager.java
Mat4 getRotationMatrix(const Vec3& gravity, const Vec3& geomagnetic)
{
float Ax = gravity.x;
float Ay = gravity.y;
float Az = gravity.z;
const float normsqA = (Ax*Ax + Ay*Ay + Az*Az);
const float g = 9.81f;
const float freeFallGravitySquared = 0.01f * g * g;
if (normsqA < freeFallGravitySquared) {
// gravity less than 10% of normal value
return Mat4::IDENTITY;
}
const float Ex = geomagnetic.x;
const float Ey = geomagnetic.y;
const float Ez = geomagnetic.z;
float Hx = Ey*Az - Ez*Ay;
float Hy = Ez*Ax - Ex*Az;
float Hz = Ex*Ay - Ey*Ax;
const float normH = std::sqrt(Hx*Hx + Hy*Hy + Hz*Hz);
if (normH < 0.1f) {
// device is close to free fall (or in space?), or close to
// magnetic north pole. Typical values are > 100.
return Mat4::IDENTITY;
}
const float invH = 1.0f / normH;
Hx *= invH;
Hy *= invH;
Hz *= invH;
const float invA = 1.0f / std::sqrt(Ax*Ax + Ay*Ay + Az*Az);
Ax *= invA;
Ay *= invA;
Az *= invA;
const float Mx = Ay*Hz - Az*Hy;
const float My = Az*Hx - Ax*Hz;
const float Mz = Ax*Hy - Ay*Hx;
return Mat4( Hx, Mx, Ax, 0,
Hy, My, Ay, 0,
Hz, Mz, Az, 0,
0, 0, 0, 1);
}
