// matrix transform from source to destination
Matrix3x3
matTransformCanonical(double translateX,
double translateY,
double scaleX,
double scaleY,
double skewX,
double skewY,
bool skewOrderYX,
double rads,
double centerX,
double centerY)
{
///1) We translate to the center of the transform.
///2) We scale
///3) We apply skewX and skewY in the right order
///4) We rotate
///5) We apply the global translation
///5) We translate back to the origin
return matMul( matMul( matMul( matMul( matMul( matTranslation(centerX, centerY),
matTranslation(translateX, translateY) ),
matRotation(-rads) ),
matSkewXY(skewX, skewY, skewOrderYX) ),
matScale(scaleX, scaleY) ),
matTranslation(-centerX, -centerY) );
}
// matrix transform from source to destination
static
Matrix3x3
matTransformPixel(double pixelaspectratio, //!< 1.067 for PAL, where 720x576 pixels occupy 768x576 in canonical coords
double renderscaleX, //!< 0.5 for a half-resolution image
double renderscaleY,
bool fielded,
double translateX,
double translateY,
double scaleX,
double scaleY,
double skewX,
double skewY,
bool skewOrderYX,
double rads,
double centerX,
double centerY)
{
///1) We go from pixel to canonical
///2) we apply transform
///3) We go back to pixels
return matMul( matMul( matCanonicalToPixel(pixelaspectratio, renderscaleX, renderscaleY, fielded),
matTransformCanonical(translateX, translateY, scaleX, scaleY, skewX, skewY, skewOrderYX, rads, centerX, centerY) ),
matPixelToCanonical(pixelaspectratio, renderscaleX, renderscaleY, fielded) );
}
void FDecomposeBasedCameraReconstructor::compute(const CvMat* F, const PointPairs& pps) {
//first transform F to nF
//K' = kH * K
//kH = [1, 0, -px; 0, 1, -py; 0, 0, 1] assuming the upleft is origin
double kh[] = {
1, 0, -principalPoint.x,
0, -1, principalPoint.y,
0, 0, 1};
CvMat kH = cvMat(3, 3, CV_64FC1, kh);
CvMat* nF = transF(F, &kH, &kH);
FDecomposer fd;
FParams fp = fd.compute(nF);
if (fd.getStatus() != FDecomposer::SUCCESS) {
cout<<"fail to decompse!"<<endl;
UnCalibratedCameraReconstructor ucr;
ucr.compute(F, pps);
cam1 = ucr.getCamera1();
cam2 = ucr.getCamera2();
} else {
FComposer fc(fp);
CvMat* K1 = matMul(&kH, fc.K1);
CvMat* K2 = matMul(&kH, fc.K2);
cam1.reset(K1, fc.R1, fc.t1);
cam2.reset(K2, fc.R2, fc.t2);
clearMats(&K1, &K2);
cout<<"params: "<<fp<<endl;
cout<<"K1: "<<K1<<endl;
cout<<"K2: "<<K2<<endl;
}
cvReleaseMat(&nF);
}
// matrix transform from destination to source
Matrix3x3
matInverseTransformCanonical(double translateX,
double translateY,
double scaleX,
double scaleY,
double skewX,
double skewY,
bool skewOrderYX,
double rads,
double centerX,
double centerY)
{
///1) We translate to the center of the transform.
///2) We scale
///3) We apply skewX and skewY in the right order
///4) We rotate
///5) We apply the global translation
///5) We translate back to the origin
// since this is the inverse, oerations are in reverse order
return matMul( matMul( matMul( matMul( matMul( matTranslation(centerX, centerY),
matScale(1. / scaleX, 1. / scaleY) ),
matSkewXY(-skewX, -skewY, !skewOrderYX) ),
matRotation(rads) ),
matTranslation(-translateX, -translateY) ),
matTranslation(-centerX, -centerY) );
}
static
Matrix3x3
matRotationAroundPoint(double rads,
double px,
double py)
{
return matMul( matTranslation(px, py), matMul( matRotation(rads), matTranslation(-px, -py) ) );
}
static
Matrix3x3
matScaleAroundPoint(double scaleX,
double scaleY,
double px,
double py)
{
return matMul( matTranslation(px, py), matMul( matScale(scaleX, scaleY), matTranslation(-px, -py) ) );
}
void fpscont(Camera* cam) {
// Rip out position
//
float vec_pos[4];
float* mat_world = cam->mat_world;
memcpy(vec_pos, &mat_world[12], sizeof(vec_pos));
memset(&mat_world[12], 0, sizeof(float) * 3);
// Mouse-look
//
short center_x = display.getWidth()/2;
short center_y = display.getHeight()/2;
short delta_x = mouse.pos[0] - center_x;
short delta_y = mouse.pos[1] - center_y;
mouse.setPos(center_x, center_y);
float mat_rotx[16];
matRot(mat_rotx, delta_y * -0.001f, 0);
matMul(mat_world, mat_rotx, mat_world);
float mat_roty[16];
matRot(mat_roty, delta_x * -0.001f, 1);
matMul(mat_roty, mat_world, mat_world);
// Movement
//
char up_down = keyboard.keyDown('w') - keyboard.keyDown('s');
char right_left = keyboard.keyDown('d') - keyboard.keyDown('a');
float speed = 0.5;
float vec_vel[4];
vec_vel[0] = right_left * speed;
vec_vel[1] = 0;
vec_vel[2] = -up_down * speed;
vec_vel[3] = 1;
matMulVec(mat_world, vec_vel, vec_vel);
vecAdd(vec_pos, vec_vel, vec_pos);
vec_pos[3] = 1;
memcpy(cam->mat_view, cam->mat_world, sizeof(float) * 16);
memcpy(&cam->mat_world[12], vec_pos, sizeof(vec_pos));
cameraWorldToView(cam);
}
void matPow(LL a[][N], int n){
LL ret[N][N];
memset(ret, 0, sizeof(ret));
for(i = 0; i < N; i++) ret[i][i] = 1;
while(n)
{
if(n & 1) matMul(ret, a, ret);
matMul(a, a, a);
n >>= 1;
}
for(i = 0; i < N; i++)
memcpy(a[i], ret[i], sizeof(a[i]));
}
PerspectiveTransform * PerspectiveTransform::accumulate(const Transform * T2) const
{
PerspectiveTransform * m = new PerspectiveTransform;
//cvMatMul(*this, *T2, *m); //don't need casts as opencv will check dims agree.
matMul(data.db, T2->data.db, m->data.db, 3, 3, 3);
return m;
}
/* main method that inverts an nxn matrix A and returns U and A inverse in its parameters
double *a = square invertible matrix A that is to be inverted
int n = dimension of A
double *u = the orthogonalized matrix U returned in this
double *b = the inverse of A returned in this
*/
void inv_double_gs(double *a, int n, double *u, double *b)
{
Matrix aMat = createMatrix(a,n);
// an nxn identity matrix
Matrix id = idMat(n);
// transpose aMat so as to run GS on its columns
transposeMatrix(aMat);
// run GS ... orthogonalizes aMat by columns
gramSchmidt(aMat,id); // aMat is now U transpose
// get G matrix
transposeMatrix(id);
//printMatrix(aMat); //print U transpose
//printMatrix(id); //print G
// A inverse = G * U transpose
Matrix ainv = initMatrix(n);
matMul(id,aMat,ainv);
//printMatrix(ainv); //print inverse of A
//return U in double *u and A inverse in double *b params
transposeMatrix(aMat);
flattenMatrix(aMat,u);
flattenMatrix(ainv,b);
// clean up all matrices
destroyMatrix(aMat);
destroyMatrix(id);
destroyMatrix(ainv);
}
bool
Matrix3x3::setHomographyFromFourPoints(const Point3D &p1,
const Point3D &p2,
const Point3D &p3,
const Point3D &p4,
const Point3D &q1,
const Point3D &q2,
const Point3D &q3,
const Point3D &q4)
{
Matrix3x3 invHp;
Matrix3x3 Hp( crossprod( crossprod(p1, p2), crossprod(p3, p4) ),
crossprod( crossprod(p1, p3), crossprod(p2, p4) ),
crossprod( crossprod(p1, p4), crossprod(p2, p3) ) );
double detHp = matDeterminant(Hp);
if (detHp == 0.) {
return false;
}
Matrix3x3 Hq( crossprod( crossprod(q1, q2), crossprod(q3, q4) ),
crossprod( crossprod(q1, q3), crossprod(q2, q4) ),
crossprod( crossprod(q1, q4), crossprod(q2, q3) ) );
double detHq = matDeterminant(Hq);
if (detHq == 0.) {
return false;
}
invHp = matInverse(Hp, detHp);
*this = matMul(Hq, invHp);
return true;
}
bool
Matrix3x3::setAffineFromThreePoints(const Point3D &p1,
const Point3D &p2,
const Point3D &p3,
const Point3D &q1,
const Point3D &q2,
const Point3D &q3)
{
Matrix3x3 invHp;
Matrix3x3 Hp(p1, p2, p3);
double detHp = matDeterminant(Hp);
if (detHp == 0.) {
return false;
}
Matrix3x3 Hq(q1, q2, q3);
double detHq = matDeterminant(Hq);
if (detHq == 0.) {
return false;
}
invHp = matInverse(Hp, detHp);
*this = matMul(Hq, invHp);
return true;
}
PerspectiveTransform * PerspectiveTransform::accumulateInverse(const Transform * T2) const
{
PerspectiveTransform * m = new PerspectiveTransform;
PerspectiveTransform T2_inv(*dynamic_cast<const PerspectiveTransform *>(T2)); //make a copy
cvInvert(T2_inv, T2_inv);
cvMatMul(T2_inv, *this, *m); //don't need casts as opencv will check dims agree.
matMul(T2_inv.data.db, data.db, m->data.db, 3, 3, 3);
return m;
}
CvMat* estimateNtd(const CvMat* K1, const CvMat* K2, const CvMat* R, const CvMat* t, const CvMat* H) {
CvMat* NH = matMul(inv(K2), H, K1);
CvMat* RmNH = sub(R, NH);
CvMat* NNH = scale(NH, 1/cvmGet(NH, 2, 2));
CvMat* NR = scale(R, 1/cvmGet(R, 2, 2));
CvMat* NRmNNH = sub(NR, NNH);
cout<<"R: "<<R<<endl;
cout<<"NH: "<<NH<<endl;
cout<<"RmNH: "<<RmNH<<endl;
cout<<"NR: "<<NR<<endl;
cout<<"NNH: "<<NNH<<endl;
cout<<"NRmNNH: "<<NRmNNH<<endl;
CvMat* Ntd = cvCreateMat(1, 3, CV_64FC1);
CvMat* c0 = getCol(NRmNNH, 0);
CvMat* c1 = getCol(NRmNNH, 1);
CvMat* c2 = getCol(NRmNNH, 2);
cvmSet(Ntd, 0, 0, estimateScale(t, c0));
cvmSet(Ntd, 0, 1, estimateScale(t, c1));
cvmSet(Ntd, 0, 2, estimateScale(t, c2));
cout<<"H: "<<H<<endl;
cout<<"recovered H: "<<matMul(K2, sub(R, matMul(t, Ntd)), inv(K1))<<endl;
cvReleaseMat(&NH);
cvReleaseMat(&RmNH);
cvReleaseMat(&c0);
cvReleaseMat(&c1);
cvReleaseMat(&c2);
return Ntd;
}
void getKinectAngle(void) {
int i;
double totalX=0, totalY=0, totalZ=0;
double tempPoint1[3];
double tempPoint2[3];
for (i=0; i<10; i++) {
// Get the raw accelerometer values and tilt data
if (freenect_sync_get_tilt_state(&tiltState, 0)) exit(1);
// Get the processed accelerometer values (calibrated to gravity)
freenect_get_mks_accel(tiltState, &dx, &dy, &dz);
totalX+=dx; totalY+=dy; totalZ+=dz;
}
dx=(totalX/10)/98;
dy=(totalY/10)/98;
dz=(totalZ/10)/98;
//dy-=0.1;
//printf("length: %lf\n", sqrt(dx*dx+dy*dy+dz*dz));
kinectAngleX = 0;//atan(dx/(sqrt(dy*dy+dz*dz)));
kinectAngleZ = 0;//atan(dy/(sqrt(dx*dx+dz*dz)))-3.14159265/2;
kinectAngleY = 0;//atan(dz/(sqrt(dx*dx+dz*dz)));
printf("pitch: %lf ", kinectAngleX*(180.0/3.14159265));
printf("yaw: %lf ", kinectAngleY*(180.0/3.14159265));
printf("roll: %lf \n", kinectAngleZ*(180.0/3.14159265));
kinectRotMat[0] = cos(kinectAngleZ)*cos(kinectAngleY);
kinectRotMat[1] = cos(kinectAngleZ)*sin(kinectAngleY)*sin(kinectAngleX) - sin(kinectAngleZ)*cos(kinectAngleX);
kinectRotMat[2] = cos(kinectAngleZ)*sin(kinectAngleY)*cos(kinectAngleX) + sin(kinectAngleZ)*sin(kinectAngleX);
kinectRotMat[3] = sin(kinectAngleZ)*cos(kinectAngleY);
kinectRotMat[4] = sin(kinectAngleZ)*sin(kinectAngleY)*sin(kinectAngleX) + cos(kinectAngleZ)*cos(kinectAngleX);
kinectRotMat[5] = sin(kinectAngleZ)*sin(kinectAngleY)*cos(kinectAngleX) - cos(kinectAngleZ)*sin(kinectAngleX);
kinectRotMat[6] = -sin(kinectAngleY);
kinectRotMat[7] = cos(kinectAngleY)*sin(kinectAngleX);
kinectRotMat[8] = cos(kinectAngleY)*cos(kinectAngleX);
tempPoint1[0]=dx;
tempPoint1[1]=dy;
tempPoint1[2]=dz;
matMul(kinectRotMat,3,3,tempPoint1,3,1,tempPoint2);
dx=tempPoint2[0];
dy=tempPoint2[1];
dz=tempPoint2[2];
//printf("accel[%lf,%lf,%lf]\n", dx,dy,dz);
}
void matLookAt(Matrix4x4 *m, vec3 camera, vec3 point, vec3 vecNorm){
vec3 forward, side, up;
Matrix4x4 mat, mat2;
memcpy(&mat2, m, sizeof(Matrix4x4));
forward.x = point.x - camera.x;
forward.y = point.y - camera.y;
forward.z = point.z - camera.z;
forward = vecNormalize(forward);
up = vecNorm;
side = vecCross(up, forward);
up = vecCross(forward, side);
side = vecNormalize(side);
up = vecNormalize(up);
// stolen from gluLookat.c
#define M(row,col) mat.m[col*4+row]
M(0,0) = side.x;
M(0,1) = side.y;
M(0,2) = side.z;
M(0,3) = 0.f;
M(1,0) = up.x;
M(1,1) = up.y;
M(1,2) = up.z;
M(1,3) = 0.f;
M(2,0) = -forward.x;
M(2,1) = -forward.y;
M(2,2) = -forward.z;
M(2,3) = 0.f;
M(3,0) = M(3,1) = M(3,2) = 0.f;
M(3,3) = 1.f;
#undef M
// -----------------------
matMul(m, &mat, &mat2);
vecMatTranslate(m, camera);
}
int main(void) {
int t;
scanf("%d",&t);
int arr[t];
int i,j;
ulli n;
for (j=0;j<t;j++){
scanf("%llu",&n);
if(n<=1259920){
ulli z;
if(n%2==0){
z=n/2;
n=z*(n+1)*(n+2);
}
else{
z=(n+1)/2;
n=n*z*(n+2);
}
printf("%llu",n);
}
else{
int a[23];
for (i = 0; i < 22; i++) {
a[i] = -1;
}
a[22] = 1;
if (n % 2 == 0) {
matMul(a, n / 2);
matMul(a, n + 1);
matMul(a, n + 2);
} else {
matMul(a, n);
matMul(a, (n + 1) / 2);
matMul(a, n + 2);
}
for (i = 0; i < 23; i++) {
if (a[i] != -1) {
printf("%d",a[i]);
}
}
}
printf("\n");
}
return 0;
}
CvMat* LineSegmentIntersectionDenormalizer::denormalize(const CvMat* nmodel, const CvMat* T1, const CvMat* T2) {
cvInv(T1, invT);
CvMat* model = matMul(invT, nmodel);
return model;
}
int main(int argc, char *argv[])
{
//choose dimension here
int dim = 20;
//allocate memory for A, U and B
double *a = malloc(dim*dim*sizeof(double));
double *u = malloc(dim*dim*sizeof(double));
double *b = malloc(dim*dim*sizeof(double));
//create a test matrix A using rand()
double *atemp = a;
int i;
for(i = 0; i < dim*dim; i++, atemp++)
{
*atemp = ((double)rand()/(double)RAND_MAX);
}
//invert matrix by double GS procedure
inv_double_gs(a,dim,u,b);
//print out matrices A, U and B (A inverse)
double *a2 = a;
double *u2 = u;
double *b2 = b;
for(i = 0; i < dim*dim; i++, a2++)
{
if(!(i % dim))
puts("");
printf("%f ",*a2);
}
puts("");
for(i = 0; i < dim*dim; i++, u2++)
{
if(!(i % dim))
puts("");
printf("%f ",*u2);
}
puts("");
for(i = 0; i < dim*dim; i++, b2++)
{
if(!(i % dim))
puts("");
printf("%f ",*b2);
}
puts("");
// check if A*B = I
Matrix amat = createMatrix(a,dim);
Matrix bmat = createMatrix(b,dim);
Matrix cmat = initMatrix(dim);
matMul(amat,bmat,cmat);
puts("");
printMatrix(cmat);
destroyMatrix(amat);
destroyMatrix(bmat);
destroyMatrix(cmat);
//free all memory
free(a);
free(u);
free(b);
return 0;
}
void triangleVertexShading(JmJob *data)
{
VertexJob v;
dmaBlockGet(&v, (uintptr_t)data->p1, sizeof(VertexJob), DmaTag);
Vec halfSizeAdd = vec(0.5f * sw, 0.5f * sh, 0, 1);
Vec halfSizeMul = vec(0.5f * sw, -0.5f * sh, 1, 0);
Vec zero = vec(0);
Mat transform;
matMul(&transform, v.projection, v.world);
while(true)
{
unsigned int k = interlockedExchangeAdd(v.counter, VerticesAtATime);
if(k >= v.end) break;
unsigned int end = std::min(k + VerticesAtATime, v.end);
unsigned int num = end - k;
unsigned int numtris = num / 3;
#ifndef PLATFORM_PS3_SPU
Triangle3D vertexShadingTris[TriangleAtATime];
#endif
dmaBlockGet(vertexShadingTris, (uintptr_t)&v.input[k], numtris * sizeof(Triangle3D), DmaTag);
for(unsigned int u = 0; u < numtris; u++)
{
const Triangle3D &tri = vertexShadingTris[u];
Triangle3D triOut[5];
Vec vtx[8];
int numTrisOut = 1;
if(g_EnableBackfaceCulling)
{
// We can't do the backface culling using a determinant of a 2x2 matrix
// in screen space because then we would have 'holes' in the output data
// therefor it happens here, in wordspace.
Vec e1 = vecSub(tri.p3, tri.p1);
Vec e2 = vecSub(tri.p2, tri.p1);
Vec n = vecCross(e1, e2);
Vec a = vecDot(v.camerapos, n);
if(vecGetElem(a, VecComponent::X) > 0) continue;
}
// perspective project
matMulVec(&triOut[0].p1, transform, tri.p1);
matMulVec(&triOut[0].p2, transform, tri.p2);
matMulVec(&triOut[0].p3, transform, tri.p3);
// cull against znear
Vec m1 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p1), zero);
Vec m2 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p2), zero);
Vec m3 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p3), zero);
Vec c2 = vecAnd(vecAnd(m1, m2), m3);
#ifdef PLATFORM_PS3
vec_uint4 ones = (vec_uint4){0xffffffff,0xffffffff,0xffffffff,0xffffffff};
if(vec_all_eq((vec_uint4)c2, ones)) continue;
#else
int result = vecMaskToInt(c2);
if(result == 15) continue; // discard, all behind nearz
#endif
#if 1
// clip triangles that intersect znear
static const int NumVerticesInATriangle = 3;
int numVertsOut = triangleClipToPlane(
(Vec*)&triOut[0],
vtx,
NumVerticesInATriangle,
vec(0, 0, 1, 0)
);
// Very simple triangulation routine
numTrisOut = 0;
for(int i = 2; i < numVertsOut; i++)
{
triOut[numTrisOut].p1 = vtx[0];
triOut[numTrisOut].p2 = vtx[i];
triOut[numTrisOut].p3 = vtx[i - 1];
numTrisOut++;
}
#endif
for(int i = 0; i < numTrisOut; i++)
{
// perspective divide
triOut[i].p1 = vecMul(triOut[i].p1, vecRcp(vecSplat<VecComponent::W>(triOut[i].p1)));
triOut[i].p2 = vecMul(triOut[i].p2, vecRcp(vecSplat<VecComponent::W>(triOut[i].p2)));
triOut[i].p3 = vecMul(triOut[i].p3, vecRcp(vecSplat<VecComponent::W>(triOut[i].p3)));
// transform to screen space
Vec r1 = vecMadd(triOut[i].p1, halfSizeMul, halfSizeAdd);
Vec r2 = vecMadd(triOut[i].p2, halfSizeMul, halfSizeAdd);
Vec r3 = vecMadd(triOut[i].p3, halfSizeMul, halfSizeAdd);
#ifdef PLATFORM_PS3_SPU
Triangle3DSetup &r = setup[s][sidx];
#else
Triangle3DSetup r;
#endif
memcpy(&r.x1, &r1, sizeof(float) * 3);
memcpy(&r.x2, &r2, sizeof(float) * 3);
memcpy(&r.x3, &r3, sizeof(float) * 3);
// deltas
r.dx1 = r.x1 - r.x2;
r.dx2 = r.x2 - r.x3;
r.dx3 = r.x3 - r.x1;
r.dy1 = r.y1 - r.y2;
r.dy2 = r.y2 - r.y3;
r.dy3 = r.y3 - r.y1;
#ifdef PLATFORM_PS3_SPU
sidx++;
if(sidx >= MaxSetupBuffered)
{
dmaWaitAll(1 << DmaListTag);
unsigned int l = interlockedExchangeAdd(v.outputCnt, sidx);
for(unsigned int u = 0; u < sidx; u++)
{
setuplist[u].notify = 0;
setuplist[u].reserved = 0;
setuplist[u].size = sizeof(Triangle3DSetup);
setuplist[u].eal = (uintptr_t)&v.output[u + l];
}
cellDmaListPut(setup[s], 0, setuplist, sizeof(setuplist), DmaListTag, 0, 0);
sidx = 0;
s ^= 1;
}
#else
unsigned int l = interlockedExchangeAdd(v.outputCnt, 1);
if(l >= MaxTrianglesDrawn) { interlockedExchangeSub(v.outputCnt, 1); break; }
else dmaBlockPut(&r, (uintptr_t)&v.output[l], sizeof(Triangle3DSetup), DmaTag);
#endif
}
}
}
#ifdef PLATFORM_PS3_SPU
if(sidx > 0)
{
dmaWaitAll(1 << DmaListTag);
unsigned int l = interlockedExchangeAdd(v.outputCnt, sidx);
for(unsigned int u = 0; u < sidx; u++)
{
setuplist[u].notify = 0;
setuplist[u].reserved = 0;
setuplist[u].size = sizeof(Triangle3DSetup);
setuplist[u].eal = (uintptr_t)&v.output[u + l];
}
cellDmaListPut(setup[s], 0, setuplist, sidx * sizeof(CellDmaListElement), DmaListTag + 1, 0, 0);
dmaWaitAll(1 << (DmaListTag + 1));
}
#endif
}
/*
** Function called to update rendering
*/
void DisplayFunc(void) {
int r, c, i, j, k, u, v;
double x, y, z;
double red, green, blue;
double tempPoint[3];
double tempPoint2[3];
//if (command==0) {
/*rawRGBFrame = freenect_sync_get_rgb_cv(0);
if (!rawRGBFrame) {
printf("Error: Kinect not connected?\n");
exit(1);
}
rawDepthFrame = freenect_sync_get_depth_cv(0);
if (!rawDepthFrame) {
printf("Error: Kinect not connected?\n");
exit(1);
}*/
getKinectAngle();
//Find XYZ points and match them to RGB image pixels
mapDepthRGB(rawDepthFrame, rawRGBFrame, undistortedDepthFrame, undistortedRGBFrame, rgbToWorld, worldToRGB);
//}
/* Clear the buffer, clear the matrix */
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
//glPushMatrix();
glTranslatef(moveX, moveY, moveZ);
glRotatef(angleX, 0, 1, 0);
glRotatef(angleY, 1, 0, 0);
//glColor3f(1, 0, 0);
//renderSphere_convenient(0,0,0,0.03,100);
/* Begins rendering the points */
glBegin(GL_POINTS);
rgbImg = (unsigned char *)undistortedRGBFrame->imageData;
for (c=10; c < 630; c++) {
for (r=10; r < 470; r++) {
i=(r*640+c);
u = worldToRGB[i*2];
v = worldToRGB[i*2+1];
tempPoint[0] = rgbToWorld[(v*640+u)*3];
tempPoint[1] = rgbToWorld[(v*640+u)*3+1];
tempPoint[2] = rgbToWorld[(v*640+u)*3+2];
matMul(kinectRotMat,3,3,tempPoint,3,1,tempPoint2);
x = tempPoint2[0];
y = -tempPoint2[1];
z = -tempPoint2[2];
red = (double)(rgbImg[(v*640 + u)*3 + 2])/255.0;
green = (double)(rgbImg[(v*640 + u)*3 + 1])/255.0;
blue = (double)(rgbImg[(v*640 + u)*3])/255.0;
glColor3f(red,green,blue);
glVertex3f(x, y, z);
}
}
//printf("totalPoints: %d\n", totalPoints);
glEnd();
/*glBegin(GL_LINES);
glColor3f(1,1,0);
glVertex3f(0, 0, 0.0);
glVertex3f(dx, dy, dz);
glColor3f(0,1,0); //green x
glVertex3f(0, 0, 0);
glVertex3f(0.1, 0, 0);
glColor3f(0,0,1); //blue y
glVertex3f(0, 0, 0);
glVertex3f(0, 0.1, 0);
glColor3f(1,0,0); //red z
glVertex3f(0, 0, 0);
glVertex3f(0, 0, -2.1);
glEnd( );*/
/* End */
glFlush();
glutSwapBuffers();
/* Update again and again */
glutPostRedisplay();
}
int main(int argc, char *argv[])
{
//double *a = malloc(dim*dim*sizeof(double));
//double a[9] = {1,0,0,0,1,0,0,0,1};
//double a[9] = {6,5,4,5,1,4,5,4,3};
//double a[16] = {1,2,3,4,5,6,3,8,9,3,4,6,3,7,1,9};
//double a[25] = {1,2,3,4,5,5,6,3,8,9,9,3,4,6,7,3,7,1,9,1,1,2,3,4,5};
//double a[9] = {1,2,3,4,5,6,3,8,9};
//double a[9] = {7.8102,4.4813,2.5607,0,-2.4327,3.0729,0,4,3};
//double a[9] = {5.4,4,7.7,3.5,-.7,2.8,-3.2,5.1,.8};
//double a[16] = {1,9,8,7,9,2,6,5,8,6,3,1,7,5,1,4};
//choose dimension of matrix here!
int n = 10;
//create a test matrix A using rand()
double *a = malloc(n*n*sizeof(double));
double *atemp = a;
int i;
for(i = 0; i < n*n; i++, atemp++)
{
*atemp = ((double)rand()/(double)RAND_MAX);
}
//allocate space for output matrices B and U
double *b = malloc(n*n*sizeof(double));
double *u = malloc(n*n*sizeof(double));
//run the main process for Hessenberg decomposition
upperhes(a,n,u,b);
//create matrix structures from input and output for testing
Matrix aMat = createMatrix(a,n);
Matrix bMat = createMatrix(b,n);
Matrix uMat = createMatrix(u,n);
Matrix uTran = createMatrix(u,n);
transposeMatrix(uTran);
//result2 = UAU^T
Matrix result = initMatrix(n);
Matrix result2 = initMatrix(n);
matMul(uMat,aMat,result);
matMul(result,uTran,result2);
puts("A = ");
printMatrix(aMat);
puts("B = ");
printMatrix(bMat);
puts("UAU^T");
printMatrix(result2);
//free malloc'ed memory before exiting
free(a);
free(b);
free(u);
destroyMatrix(aMat);
destroyMatrix(bMat);
destroyMatrix(uMat);
destroyMatrix(uTran);
destroyMatrix(result);
destroyMatrix(result2);
return 0;
}