25 #include "glfMatrix.hh"
26 #include "glfException.hh"
33 (float)m[0][0], (
float)m[0][1], (
float)m[0][2], (
float)m[0][3],
34 (
float)m[1][0], (
float)m[1][1], (
float)m[1][2], (
float)m[1][3],
35 (
float)m[2][0], (
float)m[2][1], (
float)m[2][2], (
float)m[2][3],
36 (
float)m[3][0], (
float)m[3][1], (
float)m[3][2], (
float)m[3][3]);
42 (float)m[0][0], (
float)m[0][1], (
float)m[0][2],
43 (
float)m[1][0], (
float)m[1][1], (
float)m[1][2],
44 (
float)m[2][0], (
float)m[2][1], (
float)m[2][2]);
53 0.0f, 1.0f, 0.0f, 0.0f,
54 0.0f, 0.0f, 1.0f, 0.0f,
55 0.0f, 0.0f, 0.0f, 1.0f);
62 GLfloat m4, GLfloat m5, GLfloat m6, GLfloat m7,
63 GLfloat m8, GLfloat m9, GLfloat m10, GLfloat m11,
64 GLfloat m12, GLfloat m13, GLfloat m14, GLfloat m15)
66 m[0] = m0; m[4] = m4; m[ 8] = m8; m[12] = m12;
67 m[1] = m1; m[5] = m5; m[ 9] = m9; m[13] = m13;
68 m[2] = m2; m[6] = m6; m[10] = m10; m[14] = m14;
69 m[3] = m3; m[7] = m7; m[11] = m11; m[15] = m15;
80 GLfloat ls = x*x + y*y + z*z;
81 if (fabs(ls - 1.0) > 0.00001) {
82 GLfloat l = sqrtf(ls);
83 GLfloat invL = 1.0 / l;
89 GLfloat c = cosf((angle / 180.0) * M_PI);
90 GLfloat s = sinf((angle / 180.0) * M_PI);
93 m[0] = x*x*ic+c; m[4] = y*x*ic-z*s; m[ 8] = x*z*ic+y*s; m[12] = 0.0;
94 m[1] = x*y*ic+z*s; m[5] = y*y*ic+c; m[ 9] = y*z*ic-x*s; m[13] = 0.0;
95 m[2] = x*z*ic-y*s; m[6] = y*z*ic+x*s; m[10] = z*z*ic+c; m[14] = 0.0;
96 m[3] = 0.0; m[7] = 0.0; m[11] = 0.0; m[15] = 1.0;
101 m[0] = 1.0f; m[4] = 0.0f; m[ 8] = 0.0f; m[12] = x;
102 m[1] = 0.0f; m[5] = 1.0f; m[ 9] = 0.0f; m[13] = y;
103 m[2] = 0.0f; m[6] = 0.0f; m[10] = 1.0f; m[14] = z;
104 m[3] = 0.0f; m[7] = 0.0f; m[11] = 0.0f; m[15] = 1.0f;
109 m[0] = x; m[4] = 0.0; m[ 8] = 0.0; m[12] = 0.0;
110 m[1] = 0.0; m[5] = y; m[ 9] = 0.0; m[13] = 0.0;
111 m[2] = 0.0; m[6] = 0.0; m[10] = z; m[14] = 0.0;
112 m[3] = 0.0; m[7] = 0.0; m[11] = 0.0; m[15] = 1.0;
117 GLfloat A = (right+left)/(right-left);
118 GLfloat B = (top+bottom)/(top-bottom);
119 GLfloat C = -(farVal+nearVal)/(farVal-nearVal);
120 GLfloat D = -2.0*(farVal*nearVal)/(farVal-nearVal);
122 GLfloat E = 2*nearVal/(right - left);
123 GLfloat F = 2*nearVal/(top - bottom);
125 m[0] = E; m[4] = 0.0; m[ 8] = A; m[12] = 0.0;
126 m[1] = 0.0; m[5] = F; m[ 9] = B; m[13] = 0.0;
127 m[2] = 0.0; m[6] = 0.0; m[10] = C; m[14] = D;
128 m[3] = 0.0; m[7] = 0.0; m[11] = -1.0; m[15] = 0.0;
131 void glfMatrix::MakeOrtho(GLfloat left, GLfloat right, GLfloat bottom, GLfloat top, GLfloat nearVal, GLfloat farVal)
133 GLfloat tx = -(right+left)/(right-left);
134 GLfloat ty = -(top+bottom)/(top-bottom);
135 GLfloat tz = -(farVal+nearVal)/(farVal-nearVal);
137 GLfloat a = 2.0/(right-left);
138 GLfloat b = 2.0/(top-bottom);
139 GLfloat c = -2.0/(farVal-nearVal);
141 m[0] = a; m[4] = 0.0f; m[ 8] = 0.0f; m[12] = tx;
142 m[1] = 0.0f; m[5] = b; m[ 9] = 0.0f; m[13] = ty;
143 m[2] = 0.0f; m[6] = 0.0f; m[10] = c; m[14] = tz;
144 m[3] = 0.0f; m[7] = 0.0f; m[11] = 0.0f; m[15] = 1.0f;
149 GLfloat f = 1.0 / tan(0.5 * ((fovy / 180.0) * M_PI));
151 GLfloat E = f / aspect;
153 GLfloat C = (zFar + zNear) / (zNear - zFar);
154 GLfloat D = (2.0 * zFar * zNear) / (zNear - zFar);
156 m[0] = E; m[4] = 0.0f; m[ 8] = 0.0f; m[12] = 0.0f;
157 m[1] = 0.0f; m[5] = F; m[ 9] = 0.0f; m[13] = 0.0f;
158 m[2] = 0.0f; m[6] = 0.0f; m[10] = C; m[14] = D;
159 m[3] = 0.0f; m[7] = 0.0f; m[11] = -1.0f; m[15] = 0.0f;
163 GLfloat centerX, GLfloat centerY, GLfloat centerZ,
164 GLfloat upX, GLfloat upY, GLfloat upZ)
182 a.m[0] = s[0]; a.m[4] = s[1]; a.m[ 8] = s[2]; a.m[12] = 0.0f;
183 a.m[1] = u[0]; a.m[5] = u[1]; a.m[ 9] = u[2]; a.m[13] = 0.0f;
184 a.m[2] = -f[0]; a.m[6] = -f[1]; a.m[10] = -f[2]; a.m[14] = 0.0f;
185 a.m[3] = 0.0f; a.m[7] = 0.0f; a.m[11] = 0.0f; a.m[15] = 1.0f;
195 m[0] = a[0][0]; m[4] = a[0][1]; m[ 8] = a[0][2]; m[12] = a[0][3];
196 m[1] = a[1][0]; m[5] = a[1][1]; m[ 9] = a[1][2]; m[13] = a[1][3];
197 m[2] = a[2][0]; m[6] = a[2][1]; m[10] = a[2][2]; m[14] = a[2][3];
198 m[3] = a[3][0]; m[7] = a[3][1]; m[11] = a[3][2]; m[15] = a[3][3];
208 m[0] = r[0][0]; m[4] = r[0][1]; m[ 8] = r[0][2]; m[12] = t[0];
209 m[1] = r[1][0]; m[5] = r[1][1]; m[ 9] = r[1][2]; m[13] = t[1];
210 m[2] = r[2][0]; m[6] = r[2][1]; m[10] = r[2][2]; m[14] = t[2];
211 m[3] = 0.0f; m[7] = 0.0f; m[11] = 0.0f; m[15] = 1.0f;
225 m[0] = a.m[0]; m[4] = a.m[1]; m[ 8] = a.m[ 2]; m[12] = -(x*a.m[0]+y*a.m[1]+z*a.m[2]);
226 m[1] = a.m[4]; m[5] = a.m[5]; m[ 9] = a.m[ 6]; m[13] = -(x*a.m[4]+y*a.m[5]+z*a.m[6]);
227 m[2] = a.m[8]; m[6] = a.m[9]; m[10] = a.m[10]; m[14] = -(x*a.m[8]+y*a.m[9]+z*a.m[10]);
228 m[3] = 0.0f; m[7] = 0.0f; m[11] = 0.0f; m[15] = 1.0f;
233 unsigned int uiWidth, uiHeight;
235 double width =
static_cast<double>(uiWidth);
236 double height =
static_cast<double>(uiHeight);
239 BIASASSERT(K[0][1]==0.0);
242 for(
unsigned int i =0; i<3; i++) {
243 for(
unsigned int j =0; j<3; j++) {
244 embeddedK[i][j] =
static_cast<double>(K[i][j]);
249 embeddedK[3][2] = 1.0;
251 embeddedK[0][0] /= width;
252 embeddedK[0][2] += 0.5;
253 embeddedK[0][2] /= width;
255 embeddedK[1][1] /= height;
256 embeddedK[1][2] += 0.5;
257 embeddedK[1][2] /= height;
260 embeddedK[1][1] *= -1.0;
261 embeddedK[1][2] *= -1.0;
262 embeddedK[1][2] += 1.0;
277 for(
unsigned int i=0; i<3; i++) {
284 Make(adaptedExtrinsics);
293 GLfloat nearZZ, GLfloat farZ,
bool useIdealK)
295 unsigned int uiWidth, uiHeight;
302 uiWidth, uiHeight, useIdealK);
306 GLfloat nearZ, GLfloat farZ,
bool flip)
308 unsigned int width, height;
315 GLfloat nearZ, GLfloat farZ,
316 unsigned int x0,
unsigned int y0,
317 unsigned int width,
unsigned int height,
318 bool useIdealK,
bool flip)
320 BIASASSERT(nearZ<farZ);
321 BIASASSERT(nearZ>0.0);
333 for(
unsigned int i=0; i<3; i++) {
334 for(
unsigned int j=0; j<3; j++) {
335 embededK[i][j] = K[i][j];
346 normalizingK[0][0] = 2.0/
static_cast<double>(width);
347 normalizingK[0][2] = (1.0 - 2.0*
static_cast<double>(x0))/
static_cast<double>(width) - 1.0;
349 normalizingK[1][1] = -2.0/
static_cast<double>(height);
350 normalizingK[1][2] = (2.0*
static_cast<double>(y0)-1.0)/
static_cast<double>(height) + 1.0;
352 normalizingK[1][1] = 2.0/
static_cast<double>(height);
353 normalizingK[1][2] = (1.0-2.0*
static_cast<double>(y0))/
static_cast<double>(height) - 1.0;
355 normalizingK[2][2] = (farZ + nearZ)/(farZ - nearZ);
356 normalizingK[2][3] = -(1+normalizingK[2][2])*nearZ;
358 normalizingK[3][2] = 1.0;
369 unsigned int width, height;
377 for(
unsigned int i=0; i<3; i++) {
378 for(
unsigned int j=0; j<3; j++) {
379 embededK[i][j] = K[i][j];
388 normalizingK[0][0] = 1.0/
static_cast<double>(width);
389 normalizingK[0][2] = 1.0/(2.0*
static_cast<double>(width));
391 normalizingK[1][1] = 1.0/
static_cast<double>(height);
392 normalizingK[1][2] = 1.0/(2.0*
static_cast<double>(height));
394 normalizingK[1][1] = -1.0/
static_cast<double>(height);
395 normalizingK[1][2] = 1.0-1.0/(2.0*
static_cast<double>(height));
397 normalizingK[2][2] = 1.0;
398 normalizingK[2][3] = 0;
400 normalizingK[3][2] = 1.0;
408 GLfloat nearZ, GLfloat farZ,
409 unsigned int x0,
unsigned int y0,
410 unsigned int width,
unsigned int height,
414 BIASASSERT(nearZ<farZ);
415 BIASASSERT(nearZ>0.0);
441 clippingWindowUL *= (nearZ/clippingWindowUL[2]);
442 clippingWindowLR *= (nearZ/clippingWindowLR[2]);
444 const double left = clippingWindowUL[0];
445 const double right = clippingWindowLR[0];
454 const double bottom = -clippingWindowLR[1];
455 const double top = -clippingWindowUL[1];
457 double deltaX = right - left;
458 double deltaY = top - bottom;
459 double deltaZ = nearZ - farZ;
465 res[0][0] = 2.0*nearZ / deltaX;
467 res[0][2] = (right+left)/deltaX;
469 res[1][1] = 2.0*nearZ / deltaY;
471 res[1][2] = (bottom+top)/deltaY;
472 res[2][2] = (farZ + nearZ) / (deltaZ);
473 res[2][3] = (2.0*farZ*nearZ) / (deltaZ);
482 out[0] = in[0]*m[ 0] + in[1]*m[ 4] + in[2]*m[ 8] + m[12];
483 out[1] = in[0]*m[ 1] + in[1]*m[ 5] + in[2]*m[ 9] + m[13];
484 out[2] = in[0]*m[ 2] + in[1]*m[ 6] + in[2]*m[10] + m[14];
486 GLfloat invW = 1.0 / (in[0]*m[3] + in[1]*m[7] + in[2]*m[11] + m[15]);
512 for (
int i = 0; i < 4; i++) {
513 for (
int j = 0; j < 4; j++) {
515 for (
int k = 0; k < 4; k++) {
516 c += m[i+k*4] * a.m[k+j*4];
526 return column*4 + row;
531 for(
unsigned int r=0; r<4; r++ ) {
533 for(
unsigned int c=0; c<4; c++ ) {
535 vOut[r] += m[i]*vIn[c];
virtual BIAS::Vector3< double > GetC() const
Get projection center.
glfMatrix operator*(const glfMatrix &a)
void MakeAffine(const Matrix3x3< float > &r, const Vector3< float > &t)
Initializes the render matrix as an affine mapping.
void MakeProjectionMatrixNew(const ProjectionParametersPerspective ¶ms, GLfloat nearZ, GLfloat farZ, unsigned int x0, unsigned int y0, unsigned int width, unsigned int height, bool useIdealK=false, bool flip=false)
static const glfMatrix IDENTITY
The identity matrix.
void Multiply(const glfMatrix &a)
camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
void MakeTranslation(GLfloat x, GLfloat y, GLfloat z)
glfMatrix()
The constructor leaves the matrix undefined!
void MakeLookAt(GLfloat eyeX, GLfloat eyeY, GLfloat eyeZ, GLfloat centerX, GLfloat centerY, GLfloat centerZ, GLfloat upX, GLfloat upY, GLfloat upZ)
class Vector4 contains a Vector of dim.
void SetZero()
Sets all values to zero.
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
void MakeAffineInverse(const glfMatrix &a)
Computes the inverse of the matrix, assuming it is an affine transformation with an orthonormal linea...
void Mult(const Vector4< T > &argvec, Vector4< T > &destvec) const
matrix - vector multiplicate this matrix with Vector4, storing the result in destvec, calculates: destvec = (this Matrix) * argvec
void MakeTextureMatrix(const ProjectionParametersPerspective ¶ms, bool flip=false)
Conversion from ProjectionParametersPerspective.
void MakeRotation(GLfloat angle, GLfloat x, GLfloat y, GLfloat z)
virtual BIAS::RMatrix GetR() const
Get orientation as rotation matrix R.
void Make(const Matrix4x4< float > &a)
Initializes the glf matrix from a BIAS matrix.
is a 'fixed size' quadratic matrix of dim.
void GetIdealK(KMatrix &K) const
class Vector3 contains a Vector of fixed dim.
virtual int GetImageSize(unsigned int &Width, unsigned int &Height) const
Obtain image dimensions.
void MakeTextureMatrixNew(const ProjectionParametersPerspective ¶ms, bool flip)
void MakePerspective(GLfloat fovy, GLfloat aspect, GLfloat zNear, GLfloat zFar)
virtual BIAS::KMatrix GetK() const
static unsigned int Matrix2DIndexToRep(unsigned int row, unsigned int column)
void TransformPoint(const Vector3< float > &in, Vector3< float > &out) const
Transforms a point by the matrix.
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
A 4x4 matrix in native OpenGL format.
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
void MakeViewMatrixNew(const ProjectionParametersBase ¶ms)
void MakeProjectionMatrix(const ProjectionParametersPerspective ¶ms, GLfloat nearZZ, GLfloat farZ, bool useIdealK=false)
is a 'fixed size' quadratic matrix of dim.
void MakeViewMatrix(const ProjectionParametersBase ¶ms)
void operator*=(const glfMatrix &a)
virtual void UnProjectLocal(const HomgPoint2D &pos, Vector3< double > &pointOnRay, Vector3< double > &direction, bool IgnoreDistortion=false) const
calculates the viewing ray from the camera center (in the camera coordinate system) which belongs to ...
void GetIdealImageSize(unsigned int &width, unsigned int &height) const
void MakeFrustum(GLfloat left, GLfloat right, GLfloat bottom, GLfloat top, GLfloat nearVal, GLfloat farVal)
Camera parameters which define the mapping between rays in the camera coordinate system and pixels in...
void MakeOrtho(GLfloat left, GLfloat right, GLfloat bottom, GLfloat top, GLfloat nearVal, GLfloat farVal)
virtual const BIAS::Pose & GetPose() const
Return complete pose object.
virtual void Load() const
Loads the matrix in OpenGL.
virtual void SetK(const KMatrix &K)
sets the internal parameters from a given KMatrix and updates the cached K and its inverse ...
Vector3< T > & Normalize()
normalize this vector to length 1
class BIASGeometryBase_EXPORT HomgPoint2D
void MakeScalation(GLfloat x, GLfloat y, GLfloat z)