void Camera::rotateCam(float xAngle, float yAngle)
{
float* yRMat = rotationMatrix(0.0f, 1.0f, 0.0f, xAngle);
float* xRMat = rotationMatrix(1.0f, 0.0f, 0.0f, yAngle);
multiplyMatrix(viewMatrix, yRMat);
multiplyMatrix(viewMatrix, xRMat);
}
void ObjectRenderer::setupProjection()
{
glViewport (0,0, 960, 640);
Size size(960,640);
float zeye = 640/1.1566f;
Mat4 matrixPerspective, matrixLookup;
loadIdentityMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION);
#if CC_TARGET_PLATFORM == CC_PLATFORM_WP8
//if needed, we need to add a rotation for Landscape orientations on Windows Phone 8 since it is always in Portrait Mode
GLView* view = getOpenGLView();
if(getOpenGLView() != nullptr)
{
multiplyMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION, getOpenGLView()->getOrientationMatrix());
}
#endif
// issue #1334
Mat4::createPerspective(60, (GLfloat)size.width/size.height, 1, zeye+size.height/2, &matrixPerspective);
multiplyMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION, matrixPerspective);
Vec3 eye(size.width/2, size.height/2, zeye), center(size.width/2, size.height/2, 0.0f), up(0.0f, 1.0f, 0.0f);
Mat4::createLookAt(eye, center, up, &matrixLookup);
multiplyMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_PROJECTION, matrixLookup);
loadIdentityMatrix(MATRIX_STACK_TYPE::MATRIX_STACK_MODELVIEW);
}
void fisherVerify(Matrix fisherBasis, Matrix fisherValues, Matrix Sw, Matrix Sb) {
Matrix SbW = multiplyMatrix(Sb, fisherBasis);
Matrix SwW = multiplyMatrix(Sw, fisherBasis);
Matrix D = makeIdentityMatrix(fisherBasis->row_dim);
Matrix DSwW;
Matrix zeroMat;
int i, j;
MESSAGE("Verifying Fisher Basis.");
for (i = 0; i < D->row_dim; i++) {
ME(D, i, i) = ME(fisherValues, i, 0);
}
DSwW = multiplyMatrix(D, SwW);
zeroMat = subtractMatrix(SbW, DSwW);
for (i = 0; i < zeroMat->row_dim; i++) {
for (j = 0; j < zeroMat->col_dim; j++) {
if (!EQUAL_ZERO(ME(zeroMat, i, j), 0.000001)) {
DEBUG( -1, "Fisher validation failed.");
printf("Element: (%d,%d) value = %f", i, j, ME(zeroMat, i, j));
exit(1);
}
}
}
}
void Scene::render()
{
if (!m_currentCamera.isValid()) {
return;
}
Camera* cameraComp = m_currentCamera->getCamera();
const Matrix4& matProjection = cameraComp->getProjectionMatrix();
const Matrix4& matView = cameraComp->getViewMatrix();
Matrix4 matViewProjection;
multiplyMatrix(matProjection, matView, matViewProjection);
GameObjectList& children = m_root->getChildren();
GameObjectIter iter = children.begin();
for (GameObjectIter iterEnd=children.end(); iter!=iterEnd; ++iter)
{
GameObjectPtr& obj = *iter;
const Matrix4& objMat = obj->getTransform()->getAbsoluteTransform();
Matrix4 mvp;
multiplyMatrix(matViewProjection, objMat, mvp);
obj->getTransform()->setMVPMatrix(mvp);
obj->render();
}
}
double * multipleLinearRegression(double **X, double *y, int n, int m){
double **A, **XT, **B, *b, *x, **Y;
/* int i,j; */
Y=vectorToMatrix(y,m,1);
x=allocateDoubleVector(n);
XT=trasposeMatrix(X,n,m);
/* XT.X.x = XT.b */
A=multiplyMatrix(XT,X,m,n,n);
B=multiplyMatrix(XT,Y,m,n,1);
b=matrixToVector(B,n,1);
/* for (i=0; i<n; i++){
for (j=0; j<n; j++){
fprintf(stdout,"%f ",A[i][j]);
}
fprintf(stdout,"\n");
}
for(i=0;i<n;i++) fprintf(stdout,"%f ",b[i]);
fprintf(stdout,"\n"); */
x = gaussianElimination (n, A, b);
return x;
}
void Camera::zoomCam(float zoom)
{
float zoomMat[16];
setTransMatrix(zoomMat, 0.0f, 0.0f, zoom);
multiplyMatrix(zoomMat, viewMatrix);
setIdentMatrix(viewMatrix, 4);
multiplyMatrix(viewMatrix, zoomMat);
}
/*
eigen_verify
Verify properties of the eigen vectors.
The eigenbasis should be ortonormal: R'*R - I == 0
The basis should be decomposed such that: MR - RD == 0
returns true if tests fail.
*/
int eigen_verify(Matrix M, Matrix lambda, Matrix R) {
Matrix RtR = transposeMultiplyMatrixL(R, R);
Matrix identity = makeIdentityMatrix(R->col_dim);
Matrix MR = multiplyMatrix(M, R);
Matrix D = makeIdentityMatrix(lambda->row_dim);
Matrix RD;
Matrix test;
int i, j;
int failed = 0;
const double tol = 1.0e-7;
for (i = 0; i < lambda->row_dim; i++) {
ME(D, i, i) = ME(lambda, i, 0);
}
RD = multiplyMatrix(R, D);
freeMatrix(D);
DEBUG(2, "Checking orthogonality of eigenvectors");
test = subtractMatrix(RtR, identity);
freeMatrix(RtR);
freeMatrix(identity);
for (i = 0; i < test->row_dim; i++) {
for (j = 0; j < test->col_dim; j++) {
if (!EQUAL_ZERO(ME(test, i, j), tol)) {
failed = 1;
MESSAGE("Eigenvectors are not orthogonal to within tolerance.");
DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
DEBUG_DOUBLE(1, "Tolerance", tol);
exit(1);
}
}
}
freeMatrix(test);
DEBUG(2, "Checking reconstruction property of eigensystem");
test = subtractMatrix(MR, RD);
freeMatrix(MR);
freeMatrix(RD);
for (i = 0; i < test->row_dim; i++) {
for (j = 0; j < test->col_dim; j++) {
if (!EQUAL_ZERO(ME(test, i, j), tol)) {
failed = 1;
MESSAGE("Covariance matrix is not reconstructable to within tolerance.");
DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
DEBUG_DOUBLE(1, "Tolerance", tol);
exit(1);
}
}
}
freeMatrix(test);
return failed;
}
void convertFileToDataMatrix::printMostVisitedPlace()
{
size_t largestElement = mostFindedElement(multiplyMatrix(transponateMatrix(matrixWithNumbers),matrixWithNumbers,countOfPlaces,countOfPeople),countOfPlaces);
for(size_t i = 0; i < countOfPlaces; i++)
{
for(size_t j = 0; j < countOfPlaces; j++)
{
if(largestElement == multiplyMatrix(transponateMatrix(matrixWithNumbers),matrixWithNumbers,countOfPlaces,countOfPeople)[i][j])
{
std::cout << namesOfPlaces[i] << std::endl;
}
}
}
}
void convertFileToDataMatrix::printMostConnectedMan()
{
size_t largestElement = mostFindedElement(multiplyMatrix(matrixWithNumbers,transponateMatrix(matrixWithNumbers),countOfPeople,countOfPlaces),countOfPeople);
for(size_t i = 0; i < countOfPeople; ++i)
{
for(size_t j = 0; j < countOfPeople; ++j)
{
if(largestElement == multiplyMatrix(matrixWithNumbers,transponateMatrix(matrixWithNumbers),countOfPeople,countOfPlaces)[i][j])
{
std::cout << namesOfPeople[i] << std::endl;
}
}
}
}
/*===========================================================================
* powerMethod
* This algorithm determines the largest eigenvalue of a matrix using the
* power method.
*
* This was described to me in a Randomized Algoirthms course.
*=========================================================================*/
double powerMethod(matrix* a) {
matrix* x;
matrix* xp1; // x plus 1
const double EPSILON = 0.001;
double sum;
int i;
int k = 0;
int converge;
assert(a->width == a->height, "Matrix must be square.");
srand(time(0)); // Initalize our RNG
// Let's initalize x to a random vector
x = makeMatrix(1, a->height);
for (i = 0; i < a->height; i++) {
x->data[i] = (double) rand() / RAND_MAX;
}
// Iterate until the x vector converges.
while (1) {
k++;
// Multiply A * x
xp1 = multiplyMatrix(a, x);
// Add up all of the values in xp1
sum = 0;
for (i = 0; i < a->height; i++) {
sum += xp1->data[i];
}
// Divide each value in xp1 by sum. (Normalize)
for (i = 0; i < a->height; i++) {
xp1->data[i] /= sum;
}
// Test to see if we need to quit.
converge = 1; // Converged
for (i = 0; i < a->height; i++) {
if (fabs(x->data[i] - xp1->data[i]) >= EPSILON) {
converge = 0; // Not converged.
break;
}
}
// Set up for the next loop.
freeMatrix(x);
x = copyMatrix(xp1);
freeMatrix(xp1);
// Really test for quit.
if (converge == 1) {
break;
}
}
freeMatrix(x);
return sum;
}
void renderScene(void) {
frame++;
time=glutGet(GLUT_ELAPSED_TIME);
if (MoveLight && (time - timebase2 > 10))
{
lightAngle = PI/36;
myMulti(lightPosition, rotationMatrix(0.0, 1.0, 0.0, lightAngle));
timebase2 = time;
}
if (time - timebase > 1000) {
sprintf(s,"FPS:%4.2f",
frame*1000.0/(time-timebase));
timebase = time;
frame = 0;
}
glutSetWindowTitle(s);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//placeCam(10,2,10,0,2,-5);
placeCam(viewPosition[0],viewPosition[1],viewPosition[2],0,0,-5);
multiplyMatrix(viewMatrix, rotationMatrix(0.0,1.0,0.0, angle));
multiplyMatrix(viewMatrix, rotationMatrix(1.0,0.0,0.0, angle2));
glUseProgram(p);
setUniforms();
glBindVertexArray(vert[0]);
glDrawArrays(GL_TRIANGLES, 0, sizeof(vertices1));
float T[16];
setScale(T,0.5,0.5,0.5);
multiplyMatrix(viewMatrix, T);
setTransMatrix(T,4,0,0);
multiplyMatrix(viewMatrix, T);
setUniforms();
glBindVertexArray(vert[1]);
glDrawArrays(GL_TRIANGLES, 0, sizeof(vertices1));
glutSwapBuffers();
}
void Renderable::setMatrix() {
#ifdef OPENGL_ES1
glTranslatef(posx, posy, posz);
glRotatef(rot, rotx, roty, rotz);
glScalef(scalex, scaley, scalez);
#else
Mat16 tmp = translationMatrix(posx, posy, posz);
currentModelViewMatrix = multiplyMatrix(currentModelViewMatrix, tmp);
tmp = rotationMatrix(rot / 180 * M_PI, rotx, roty, rotz);
currentModelViewMatrix = multiplyMatrix(currentModelViewMatrix, tmp);
tmp = scaleMatrix(scalex, scaley, scalez);
currentModelViewMatrix = multiplyMatrix(currentModelViewMatrix, tmp);
glUniformMatrix4fv(shaderModelViewMatrix, 1, GL_FALSE, currentModelViewMatrix.m);
glUniformMatrix3fv(shaderNormalMatrix, 1, GL_FALSE, transposedInverseMatrix9(currentModelViewMatrix).m);
#endif
}
void convertFileToDataMatrix::printFWresult()
{
floydWarshall(
multiplyMatrix(
matrixWithNumbers,transponateMatrix(matrixWithNumbers),
countOfPeople,
countOfPlaces),
countOfPeople);
}
// Function composed by other 10 functions
double compose(double C, double *lambda, double *d, double *x, double ***M, double (**func)(double *x)) {
double w[10];
double fit[10];
double z[n];
double xl[n];
double y[n];
double yl[n];
for (int i = 0; i < 10; i++)
y[i] = 5;
for (int i = 0; i < 10; i++) {
double sum = sphere(x);
w[i] = exp(-sum / (2*n*d[i]));
multiplyScalar(x, 1/lambda[i], xl);
multiplyMatrix(xl, M[i], z);
fit[i] = func[i](z);
multiplyScalar(y, 1/lambda[i], yl);
multiplyMatrix(yl, M[i], z);
double fmax = func[i](z);
fit[i] = C * fit[i] / fmax;
}
double sw = 0;
double maxw = 0;
for (int i = 0; i < 10; i++) {
sw += w[i];
maxw = max(maxw, w[i]);
}
for (int i = 0; i < 10; i++) {
if (w[i] != maxw)
w[i] = w[i] * (1 - pow(maxw, 10));
w[i] /= sw;
}
double f = 0;
for (int i = 0; i < 10; i++)
f += w[i] * fit[i];
return f;
}
/**
eigentrain
computes the eigen space for matrix images.
This function is used in the training procedure of the face recognition pca
algorithm.
INPUT: the data matrix of images
OUTPUT: mean: the mean value of the images
eigen_values: eigenvalues
eigen_base: eigenvectors
The data matrix is mean centered, and this is a side effect.
*/
void eigentrain(Matrix *mean, Matrix *eigen_vals,
Matrix *eigen_base, Matrix images)
{
double p = 0.0;
Matrix M, eigenvectors;
DEBUG(1, "Calculating mean image.");
*mean = get_mean_image(images);
DEBUG(1, "Calculating the mean centered images for the training set.");
mean_subtract_images(images, *mean);
MESSAGE2ARG("Calculating Covariance Matrix: M = images' * images. M is a %d by %d Matrix.", images->col_dim, images->col_dim);
M = transposeMultiplyMatrixL(images, images);
DEBUG_INT(3, "Covariance Matrix Rows", M->row_dim);
DEBUG_INT(3, "Covariance Matrix Cols", M->col_dim);
DEBUG(2, "Allocating memory for eigenvectors and eigenvalues.");
eigenvectors = makeMatrix(M->row_dim, M->col_dim);
*eigen_vals = makeMatrix(M->row_dim, 1);
MESSAGE("Computing snap shot eigen vectors using the double precision cv eigensolver.");
cvJacobiEigens_64d(M->data, eigenvectors->data, (*eigen_vals)->data, images->col_dim, p, 1);
freeMatrix(M);
DEBUG(1, "Verifying the eigen vectors");
/* Reconstruct M because it is destroyed in cvJacobiEigens */
M = transposeMultiplyMatrixL(images, images);
if (debuglevel >= 3)
eigen_verify(M, *eigen_vals, eigenvectors);
freeMatrix(M);
*eigen_base = multiplyMatrix(images, eigenvectors);
MESSAGE2ARG("Recovered the %d by %d high resolution eigen basis.", (*eigen_base)->row_dim, (*eigen_base)->col_dim);
DEBUG(1, "Normalizing eigen basis");
basis_normalize(*eigen_base);
/*Remove last elements because they are unneeded. Mean centering the image
guarantees that the data points define a hyperplane that passes through
the origin. Therefore all points are in a k - 1 dimensional subspace.
*/
(*eigen_base)->col_dim -= 1;
(*eigen_vals)->row_dim -= 1;
eigenvectors->col_dim -= 1;
DEBUG(1, "Verifying eigenbasis");
if (debuglevel >= 3)
basis_verify(images, *eigen_base);
/* The eigenvectors for the smaller covariance (snap shot) are not needed */
freeMatrix(eigenvectors);
}
Matrix leastSquares(Matrix A, Matrix b) {
int prealloc = alloc_matrix;
Matrix At = transposeMatrix(A);
Matrix AtA = transposeMultiplyMatrixL(A,A);
Matrix AtAi = invertRREF(AtA);
Matrix Atb = multiplyMatrix(At,b);
Matrix a = multiplyMatrix(AtAi,Atb);
freeMatrix(At);
freeMatrix(AtA);
freeMatrix(AtAi);
freeMatrix(Atb);
if(prealloc != alloc_matrix - 1) {
printf("Error deallocating matricies <%s>: pre=%d post=%d",__FUNCTION__, prealloc, alloc_matrix);
exit(1);
}
return a;
}
Matrix generateTransform(const TwoPoints *source, const TwoPoints *dest, int reflect){
double sourceMidX = (source->x1 + source->x2)*0.5;
double sourceMidY = (source->y1 + source->y2)*0.5;
double destMidX = (dest->x1 + dest->x2)*0.5;
double destMidY = (dest->y1 + dest->y2)*0.5;
Matrix translate1 = translateMatrix(-sourceMidX, -sourceMidY); /* translate the left point to the origin */
Matrix translate2 = translateMatrix(destMidX, destMidY); /* translate the origin to the left destination */
/* compute the scale that needs to be applyed to the image */
double sdist = sqrt(SQR(source->x1 - source->x2) + SQR(source->y1 - source->y2));
double ddist = sqrt(SQR(dest->x1 - dest->x2) + SQR(dest->y1 - dest->y2));
double s = ddist/sdist;
Matrix scale = scaleMatrix(s);
/* compute the rotation that needs to be applyed to the image */
double stheta = atan((source->y2 -source->y1)/(source->x2 - source->x1));
double dtheta = atan((dest->y2 -dest->y1)/(dest->x2 - dest->x1));
double theta = dtheta - stheta;
Matrix rotate = rotateMatrix(theta);
/* compute the reflection if nessicary */
Matrix reflection = reflectMatrix(reflect,0);
/* build the final transformation */
Matrix tmp1 = multiplyMatrix(scale, translate1);
Matrix tmp2 = multiplyMatrix(rotate, tmp1);
Matrix tmp3 = multiplyMatrix(reflection, tmp2);
Matrix transform = multiplyMatrix(translate2,tmp3);
/* free temporary matricies */
freeMatrix(translate1);
freeMatrix(translate2);
freeMatrix(scale);
freeMatrix(rotate);
freeMatrix(reflection);
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
/* return final transformation */
return transform;
}
void Frustum::calculate()
{
multiplyMatrix();
Plane *p;
// right
p = &plane[0];
p->normal.x = frustumMatrix[3] - frustumMatrix[0];
p->normal.y = frustumMatrix[7] - frustumMatrix[4];
p->normal.z = frustumMatrix[11] - frustumMatrix[8];
p->d = frustumMatrix[15] - frustumMatrix[12];
// left
p = &plane[1];
p->normal.x = frustumMatrix[3] + frustumMatrix[0];
p->normal.y = frustumMatrix[7] + frustumMatrix[4];
p->normal.z = frustumMatrix[11] + frustumMatrix[8];
p->d = frustumMatrix[15] + frustumMatrix[12];
// bottom
p = &plane[2];
p->normal.x = frustumMatrix[3] + frustumMatrix[1];
p->normal.y = frustumMatrix[7] + frustumMatrix[5];
p->normal.z = frustumMatrix[11] + frustumMatrix[9];
p->d = frustumMatrix[15] + frustumMatrix[13];
// top
p = &plane[3];
p->normal.x = frustumMatrix[3] - frustumMatrix[1];
p->normal.y = frustumMatrix[7] - frustumMatrix[5];
p->normal.z = frustumMatrix[11] - frustumMatrix[9];
p->d = frustumMatrix[15] - frustumMatrix[13];
// far
p = &plane[4];
p->normal.x = frustumMatrix[3] - frustumMatrix[2];
p->normal.y = frustumMatrix[7] - frustumMatrix[6];
p->normal.z = frustumMatrix[11] - frustumMatrix[10];
p->d = frustumMatrix[15] - frustumMatrix[14];
//near
p = &plane[5];
p->normal.x = frustumMatrix[3] + frustumMatrix[2];
p->normal.y = frustumMatrix[7] + frustumMatrix[6];
p->normal.z = frustumMatrix[11] + frustumMatrix[10];
p->d = frustumMatrix[15] + frustumMatrix[14];
// Normalize all plane normals
for (int i = 0 ; i < 6 ; i++)
{
plane[i].normalize();
}
}
// Shifted rotated Rastrigin’s function: F10 from [43]
double f9(double *x) {
double f = 0;
double z[n];
double tmp[n];
subtractVector(x, O, tmp);
multiplyMatrix(tmp, A, z);
for (int i = 0; i < n; i++)
f += z[i]*z[i] - 10*cos(2 * M_PI * z[i]) + 10;
return f;
}
void loadMatrix(GLfloat* newMatrix) {
GLfloat* topMatrix = getMatrixStackTop();
GLfloat* newTopMatrix = new GLfloat[16];
multiplyMatrix(topMatrix, newMatrix, newTopMatrix);
myGlMatrixStack.pop();
myGlMatrixStack.push(newTopMatrix);
glLoadMatrixf(newTopMatrix);
delete[] topMatrix;
}
int main(void) {
int intArrA[] = {1,2,3,4,5,6,7,8,9};
int intArrB[] = {1,2,3,3,2,1,2,1,3};
Matrix *ma = createMatrix(3, 3, intArrA, 9);
Matrix *mb = createMatrix(3, 3, intArrB, 9);
multiplyMatrix(ma, mb);
destroyMatrix(ma);
destroyMatrix(mb);
return 0;
}
// Apply a rotation
void
rotatePoseMatrix(float angle, float x, float y, float z,
float* matrix)
{
if (matrix) {
float rotate_matrix[16];
setRotationMatrix(angle, x, y, z, rotate_matrix);
// matrix * scale_matrix
multiplyMatrix(matrix, rotate_matrix, matrix);
}
}
void convergeMatrix(Matrix *matrix) {
Matrix *originalCopy = copyMatrix(matrix);
Matrix *copy;
int n = 1;
do {
copy = copyMatrix(matrix);
multiplyMatrix(matrix, copy, originalCopy);
n++;
} while(keepConverging(n, matrix, copy));
}
void kalmanProcess(kalman_s *mykalman)
{
int i,j;
for(i=0;i<NM;i++)
memset(mykalman->temp_4_4[i],0,sizeof(double)*NM);
//第一个公式：x(k|k-1) = Ax(k-1|k-1)
multiplyMatrix(mykalman->A,NS,NS,mykalman->X,NS,1,mykalman->temp_1); //X=A*X
for (i=0;i<NS;i++)
mykalman->X[i][0]=mykalman->temp_1[i][0];
//第二个公式： P = A*P*A'+Q
multiplyMatrix(mykalman->A,NS,NS,mykalman->P,NS,NS,mykalman->temp_2_1); //temp_2_1 = A*P
transpositionMatrix(mykalman->A, mykalman->temp_2, NS, NS); //temp_2 = A'
multiplyMatrix(mykalman->temp_2_1,NS,NS,mykalman->temp_2,NS,NS,mykalman->P); //P = A*P*A’
addMatrix(mykalman->P,NS,NS,mykalman->Q,NS,NS,mykalman->P); //P = A*P*A’+Q
//第三个公式： X = X+K*[Z-H*X]
multiplyMatrix(mykalman->H,NM,NS,mykalman->X,NS,1,mykalman->temp_3_1); //temp_3_1=H*X
subMatrix(mykalman->Z,NM,1,mykalman->temp_3_1,NM,1,mykalman->temp_3_1); //temp_3_1=Z-H*X
multiplyMatrix(mykalman->K,NS,NM,mykalman->temp_3_1,NM,1,mykalman->temp_3_2); //temp_3_2 = K*(Z-H*X)
addMatrix(mykalman->X,NS,1,mykalman->temp_3_2,NS,1,mykalman->X); //X = X+ K*(Z-H*X)
//第四个公式：K = P*H'/[H*P*H'+R]
transpositionMatrix(mykalman->H,mykalman->temp_4_3,NM,NS); //temp_4_3 = H'
multiplyMatrix(mykalman->P,NS,NS,mykalman->temp_4_3,NS,NM,mykalman->temp_4_1); //temp_4_1 = P*H'
multiplyMatrix(mykalman->H,NM,NS,mykalman->temp_4_1,NS,NM,mykalman->temp_4_2); //temp_4_2 =H*P*H'
addMatrix(mykalman->temp_4_2,NM,NM,mykalman->R,NM,NM,mykalman->temp_4_2); //temp_4_2 =H*P*H'+R
InverseMatrix(mykalman->temp_4_2, mykalman->temp_4_4, NM,NM); //temp_4_4=~(H*P*H'+R)
multiplyMatrix(mykalman->temp_4_1,NS,NM,mykalman->temp_4_4,NM,NM,mykalman->K); //K = P*H'*~(H*P*H'+R)
//第五个公式：P = [I-K*H]*P
multiplyMatrix(mykalman->K,NS,NM,mykalman->H,NM,NS,mykalman->temp_5); //temp_5 = K*H
subMatrix(mykalman->E,NS,NS,mykalman->temp_5,NS,NS,mykalman->temp_5_1); //temp_5_1 = E-K*H
multiplyMatrix(mykalman->temp_5_1,NS,NS,mykalman->P,NS,NS,mykalman->temp_5_2); //temp_5_2 = (E-K*H)*P
for (i=0;i<NS;i++)
for (j=0;j<NS;j++)
mykalman->P[i][j] = mykalman->temp_5_2[i][j];
}
Matrix weightedLeastSquares(Matrix A, Matrix b, Matrix W) {
int prealloc = alloc_matrix;
Matrix WA = multiplyMatrix(W,A);
Matrix WAt = transposeMatrix(WA);
Matrix WAtW = multiplyMatrix(WAt,W);
Matrix WAtWA = multiplyMatrix(WAtW,A);
Matrix WAtWAi = invertRREF(WAtWA);
Matrix WAtWAiWAt = multiplyMatrix(WAtWAi,WAt);
Matrix WAtWAiWAtW = multiplyMatrix(WAtWAiWAt,W);
Matrix a = multiplyMatrix(WAtWAiWAtW,b);
freeMatrix(WA);
freeMatrix(WAt);
freeMatrix(WAtW);
freeMatrix(WAtWA);
freeMatrix(WAtWAi);
freeMatrix(WAtWAiWAt);
freeMatrix(WAtWAiWAtW);
if(prealloc != alloc_matrix - 1) {
printf("Error deallocating matricies <%s>: pre=%d post=%d",__FUNCTION__, prealloc, alloc_matrix);
exit(1);
}
return a;
}
// View Matrix
// just like glulookat
void placeCam(float posX, float posY, float posZ, float lookX, float lookY, float lookZ)
{
float dir[3], right[3], up[3];
up[0] = 0.0f; up[1] = 1.0f; up[2] = 0.0f;
dir[0] = (lookX - posX);
dir[1] = (lookY - posY);
dir[2] = (lookZ - posZ);
normalize(dir);
// this order matters
xProduct(dir,up,right);
normalize(right);
xProduct(right,dir,up);
normalize(up);
float aux[16];
viewMatrix[0] = right[0];
viewMatrix[1] = up[0];
viewMatrix[2] = -dir[0];
viewMatrix[3] = 0.0f;
viewMatrix[4] = right[1];
viewMatrix[5] = up[1];
viewMatrix[6] = -dir[1];
viewMatrix[7] = 0.0f;
viewMatrix[8] = right[2];
viewMatrix[9] = up[2];
viewMatrix[10] = -dir[2];
viewMatrix[11] = 0.0f;
viewMatrix[12] = 0.0f;
viewMatrix[13] = 0.0f;
viewMatrix[14] = 0.0f;
viewMatrix[15] = 1.0f;
normalMatrix[0] = viewMatrix[0];
normalMatrix[1] = viewMatrix[1];
normalMatrix[2] = viewMatrix[2];
normalMatrix[3] = viewMatrix[4];
normalMatrix[4] = viewMatrix[5];
normalMatrix[5] = viewMatrix[6];
normalMatrix[6] = viewMatrix[8];
normalMatrix[7] = viewMatrix[9];
normalMatrix[8] = viewMatrix[10];
setTransMatrix(aux, -posX, -posY, -posZ);
multiplyMatrix(viewMatrix, aux);
}
/**
Verify properties of the eigen basis used for pca.
The eigenbasis should be ortonormal: U'*U - I == 0
The basis should be decomposed such that: X == U*D*V'
returns true if tests fail.
*/
int basis_verify(Matrix X, Matrix U) {
Matrix UtX = transposeMultiplyMatrixL(U, X);
Matrix UUtX = multiplyMatrix(U, UtX);
Matrix UtU = transposeMultiplyMatrixL(U, U);
Matrix identity = makeIdentityMatrix(U->col_dim);
Matrix test;
int i, j;
int failed = 0;
const double tol = 1.0e-7;
freeMatrix(UtX);
DEBUG(2, "Checking orthogonality of eigenbasis");
test = subtractMatrix(UtU, identity);
freeMatrix(UtU);
freeMatrix(identity);
for (i = 0; i < test->row_dim; i++) {
for (j = 0; j < test->col_dim; j++) {
if (!EQUAL_ZERO(ME(test, i, j), tol)) {
failed = 1;
MESSAGE("Eigenbasis is not orthogonal to within tolerance.");
DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
DEBUG_DOUBLE(1, "Tolerance", tol);
exit(1);
}
}
}
freeMatrix(test);
DEBUG(2, "Checking reconstruction property of the eigen decomposition");
test = subtractMatrix(X, UUtX);
freeMatrix(UUtX);
for (i = 0; i < test->row_dim; i++) {
for (j = 0; j < test->col_dim; j++) {
if (!EQUAL_ZERO(ME(test, i, j), tol)) {
failed = 1;
MESSAGE("Data matrix is not reconstructable to within tolerance.");
DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
DEBUG_DOUBLE(1, "Tolarence", tol);
exit(1);
}
}
}
freeMatrix(test);
return failed;
}
void readAccelerometerData(void){
float readings[3];
float* calibrated_matrix;
float* kalman_output;
float Ax, Ay,Az;
LIS3DSH_ReadACC(readings);
kalman_output = multiplyMatrix(readings);
readings[0] = kalmanFilter(readings[0],&kSx);
readings[1] = kalmanFilter(readings[1],&kSy);
readings[2] = kalmanFilter(readings[2],&kSz);
calibrated_matrix = multiplyMatrix(readings);
Ax = calibrated_matrix[0];
Ay = calibrated_matrix[1];
Az = calibrated_matrix[2];
Pitch = atan2(Ax,Az) * 180 / 3.1415926515;
Roll = atan2(Ay,Az)* 180 / 3.1415926515;
//printf("Pitch = %f \n", Pitch);
}
// Shifted rotated Weierstrass function: F11 from [43]
double f19(double *x) {
double f = 0;
double a = 0.5;
double b = 3.0;
double z[n];
double tmp[n];
int kmax = 20;
subtractVector(x, O, tmp);
multiplyMatrix(tmp, A, z);
f = weierstrass(z);
for (int k = 0; k <= kmax; k++)
f -= n * pow(a, k) * cos(2 * M_PI * pow(b, k) * 0.5);
return f;
}
// View Matrix
// just like glulookat
void Camera::placeCam(float posX, float posY, float posZ, float lookX, float lookY, float lookZ)
{
float dir[3], right[3], up[3];
up[0] = 0.0f; up[1] = 1.0f; up[2] = 0.0f;
dir[0] = (lookX - posX);
dir[1] = (lookY - posY);
dir[2] = (lookZ - posZ);
normalize(dir);
//zoom
//posX += dir[0]
// this order matters
xProduct(dir,up,right);
normalize(right);
xProduct(right,dir,up);
normalize(up);
float aux[16];
viewMatrix[0] = right[0];
viewMatrix[1] = up[0];
viewMatrix[2] = -dir[0];
viewMatrix[3] = 0.0f;
viewMatrix[4] = right[1];
viewMatrix[5] = up[1];
viewMatrix[6] = -dir[1];
viewMatrix[7] = 0.0f;
viewMatrix[8] = right[2];
viewMatrix[9] = up[2];
viewMatrix[10] = -dir[2];
viewMatrix[11] = 0.0f;
viewMatrix[12] = 0.0f;
viewMatrix[13] = 0.0f;
viewMatrix[14] = 0.0f;
viewMatrix[15] = 1.0f;
setTransMatrix(aux, -posX, -posY, -posZ);
multiplyMatrix(viewMatrix, aux);
// you should do this instead. If you want to apply rotation to your viewMatrix.
//multiplyMatrix(viewMatrix, rotationMatrix(0.0,0.0,1.0, 0.785));
}