/**
@param[in] vetrices array of vetrices (so far only for 3)
@param[in] E material constatn E
@param[in] mi material constatn mi
@param[in] fs volume force vector
@param[out] stifMat stifness matrix (array 36 long)
@param[out]
*/
void elastLoc(Point *vetrices[], PetscReal E, PetscReal mi, PetscReal *fs,
PetscReal *stifMat, PetscReal *bL) {
PetscReal
T[] =
{ 1, 1, 1, vetrices[0]->x, vetrices[1]->x, vetrices[2]->x, vetrices[0]->y, vetrices[1]->y, vetrices[2]->y };
PetscReal phiGrad[] = { vetrices[1]->y - vetrices[2]->y, vetrices[2]->x
- vetrices[1]->x, vetrices[2]->y - vetrices[0]->y, vetrices[0]->x
- vetrices[2]->x, vetrices[0]->y - vetrices[1]->y, vetrices[1]->x
- vetrices[0]->x
};
//PetscPrintf(PETSC_COMM_SELF, "DET:%e \n", matrixDet(T, 3));
PetscReal detT = fabs(matrixDet(T, 3));
PetscReal phiGradT[6];
matrixTranspose(phiGrad, 3, 2, phiGradT);
double Cmu[] = { 2, 0, 0, 0, 2, 0, 0, 0, 1 };
//double Clm[] = { 1, 1, 0, 1, 1, 0, 0, 0, 0 }; Why is this never used?
PetscReal C[] = { 1 - mi, mi, 0, mi, 1 - mi, 0, 0, 0, (1 - 2 * mi) / 2 };
matrixSum(C, E / ((1 + mi) * (1 - 2 * mi)), Cmu, 0, C, 3, 3);
double R[18], RT[18];
for (int i = 0; i < 18; i++)
R[i] = 0;
int idxR1[] = { 0, 2 };
int idxC1[] = { 0, 2, 4 };
int idxR2[] = { 2, 1 };
int idxC2[] = { 1, 3, 5 };
matrixSetSub(R, 3, 6, idxR1, 2, idxC1, 3, phiGradT);
matrixSetSub(R, 3, 6, idxR2, 2, idxC2, 3, phiGradT);
matrixTranspose(R, 3, 6, RT);
PetscReal temp[18];
matrixMult(C, 3, 3, R, 3, 6, temp);
matrixMult(RT, 6, 3, temp, 3, 6, stifMat);
matrixSum(stifMat, 1 / (2 * detT), stifMat, 0, stifMat, 6, 6);
//PetscPrintf(PETSC_COMM_SELF, "%e %e \n", fs[0], fs[1]);
//Volume Force
for (int i = 0; i < 3; i++) {
bL[i * 2] = detT / 6 * fs[0];
bL[i * 2 + 1] = detT / 6 * fs[1];
}
//@TODO Edge Force!!!
}
void Regulate(float *Coord_cur, float *Speed_cur, float *tAlphZad,float *V_etalon,float *alphZad, float *V_local)
{
float t_alph_cur[2][2], t_alph_delta[2][2];
float uS, uE, u[2];
float buf[2][2],buf1[2][2],buf3[2][1];
float cosAlphCur;
float sinAlphCur;
float resultVector = sqrtf((*Coord_cur) * (*Coord_cur) + (*(Coord_cur+1)) * (*(Coord_cur+1)));
if (resultVector==0.0)
{
cosAlphCur=cosf(*alphZad);
sinAlphCur=sinf(*alphZad);
}
else
{
cosAlphCur = (*Coord_cur)/resultVector;
sinAlphCur = (*(Coord_cur+1))/resultVector;
}
t_alph_cur[0][0] = cosAlphCur;
t_alph_cur[0][1] = sinAlphCur;
t_alph_cur[1][0] = -sinAlphCur;
t_alph_cur[1][1] = cosAlphCur;
matrixTranspose(&t_alph_cur[0][0], 2, 2, &buf[0][0]); //tDeltaAlph=tAlphCur'
matrixMultiplyM2M(tAlphZad, 2, 2,&buf[0][0],2,2,&t_alph_delta[0][0]);
matrixTranspose(&t_alph_delta[0][0], 2, 2, &buf1[0][0]); // We have current traectory error in buf1 now
////////////////////////////////////////////////////////////////////////////////
//radSpeed.current = (*(Speed_cur+2));
//radSpeed.target = *(V_etalon+1);//controlBoard.x2;
//pidCalc(&radSpeed);
*(V_local+2) =*(V_etalon+1);//// radSpeed.output;
////////////////////////////////////////////////////////////////////////////////
//trackSpeed.current = *(Speed_cur);
//trackSpeed.target = *(V_etalon);//controlBoard.x2;
//pidCalc(&trackSpeed);
//uS = trackSpeed.output;
////////////////////////////////////////////////////////////////////////////////
ortoPos.current = (*(Coord_cur+1));
ortoPos.target = 0.0;//controlBoard.x2;
pidCalc(&ortoPos);
uE = ortoPos.output;
////////////////////////////////////////////////////////////////////////////////
uS =(*V_etalon);
//uE = KOFF_ORTO_TRACE * (*(Coord_cur+1));
u[0] = uS;
u[1] = uE;
matrixMultiplyM2M(&buf1[0][0], 2, 2, &u[0], 2, 1, &buf3[0][0]);
matrixMultiplyM2M(&buf[0][0],2,2,&buf3[0][0],2,1,V_local);
}
int main()
{
printf("Testing work:\n");
int n = 9;
int m = 1;
Matrix a = matrixNew(n, m);
for (int i = 0; i < matrixGetRows(a); i++)
for (int j = 0; j < matrixGetCols(a); j++)
matrixSet(a, i, j, (float)(i + 1) / (j + 1));
for (int i = 0; i < matrixGetRows(a); i++)
{
for (int j = 0; j < matrixGetCols(a); j++)
printf("%.5f ", matrixGet(a, i, j));
printf("\n");
}
printf("\n");
Matrix b = matrixScale(a, 10);
for (int i = 0; i < matrixGetRows(b); i++)
{
for (int j = 0; j < matrixGetCols(b); j++)
printf("%.5f ", matrixGet(b, i, j));
printf("\n");
}
printf("\n");
Matrix c = matrixAdd(a, b);
for (int i = 0; i < matrixGetRows(c); i++)
{
for (int j = 0; j < matrixGetCols(c); j++)
printf("%.5f ", matrixGet(c, i, j));
printf("\n");
}
printf("\n");
Matrix d = matrixTranspose(c);
for (int i = 0; i < matrixGetRows(d); i++)
{
for (int j = 0; j < matrixGetCols(d); j++)
printf("%.5f ", matrixGet(d, i, j));
printf("\n");
}
/*c = matrixAdd(a, matrixNew(1, 1));
if (c == NULL)
printf("Yeah\n"), c = matrixNew(3, 3);*/
Matrix e = matrixMul(c, d);
printf("%i %i\n", matrixGetRows(e), matrixGetCols(e));
for (int i = 0; i < matrixGetRows(e); i++)
{
for (int j = 0; j < matrixGetCols(e); j++)
printf("%.5f ", matrixGet(e, i, j));
printf("\n");
}
matrixDelete(a);
matrixDelete(b);
matrixDelete(c);
matrixDelete(d);
matrixDelete(e);
return 0;
}
void matrixNormalTransform(matrix* r, matrix m) {
m.v[3] = 0;
m.v[7] = 0;
m.v[11] = 0;
m.v[15] = 1;
matrix t;
matrixInverse( &t, m );
matrixTranspose( r, t );
}
void Frustum::Extract(const Matrix44f &view, const Projection &proj) {
static const int LBN = kLBNCorner, LTN = kLTNCorner, LBF = kLBFCorner,
LTF = kLTFCorner, RBN = kRBNCorner, RTN = kRTNCorner,
RBF = kRBFCorner, RTF = kRTFCorner;
const float lnear = -proj.get_near();
const float lfar = -proj.get_far();
const Vec3f nearNorm(0.0f, 0.0f, -1.0f);
const Vec3f farNorm(0.0f, 0.0f, 1.0f);
// kLeftPlane plane
const Vec3f ltn(proj.get_left(), proj.get_top(), lnear),
lbn(proj.get_left(), proj.get_bottom(), lnear),
ltf(proj.get_far_left(), proj.get_far_top(), lfar),
lbf(proj.get_far_left(), proj.get_far_bottom(), lfar);
const Vec3f leftNorm = vecNormal(
vecCross(lbn - ltn, ltf - ltn));
// kRightPlane plane
const Vec3f rtn(proj.get_right(), proj.get_top(), lnear), // right top near
rbn(proj.get_right(), proj.get_bottom(), lnear), // right bottom near
rtf(proj.get_far_right(), proj.get_far_top(), lfar); // right top far
const Vec3f rightNorm = vecNormal(vecCross(rtf - rtn, rbn - rtn));
// kTopPlane plane
const Vec3f topNorm = vecNormal(vecCross(ltf - ltn, rtn - ltn));
// kBottomPlane plane lbn rbf
const Vec3f rbf(proj.get_far_right(), proj.get_far_bottom(), lfar);
const Vec3f botNorm = vecNormal(vecCross(rbf - rbn, lbn - rbn));
Matrix44f viewSpace, viewSpaceN;
matrixInverse(view, &viewSpace);
viewSpaceN = matrixTranspose(view);
m_conners[LBN] = lbn * viewSpace;
m_conners[LTN] = ltn * viewSpace;
m_conners[LBF] = lbf * viewSpace;
m_conners[LTF] = ltf * viewSpace;
m_conners[RBN] = rbn * viewSpace;
m_conners[RTN] = rtn * viewSpace;
m_conners[RBF] = rbf * viewSpace;
m_conners[RTF] = rtf * viewSpace;
m_planes[kLeftPlane] = Planef(leftNorm * viewSpaceN, m_conners[LTN]);
m_planes[kRightPlane] = Planef(rightNorm * viewSpaceN, m_conners[RTN]);
m_planes[kTopPlane] = Planef(topNorm * viewSpaceN, m_conners[RTN]);
m_planes[kBottomPlane] = Planef(botNorm * viewSpaceN, m_conners[LBN]);
m_planes[kNearPlane] = Planef(nearNorm * viewSpaceN, m_conners[LBN]);
m_planes[kFarPlane] = Planef(farNorm * viewSpaceN, m_conners[RTF]);
}
static void
loadTextureProjection(int texUnit, GLfloat m[16])
{
GLfloat mInverse[4][4];
/* Should use true inverse, but since m consists only of rotations, we can
just use the transpose. */
matrixTranspose((GLfloat *) mInverse, m);
ActiveTexture(GL_TEXTURE0_ARB + texUnit);
glMatrixMode(GL_TEXTURE);
glLoadIdentity();
glTranslatef(0.5, 0.5, 0.0);
glScalef(0.5, 0.5, 1.0);
glFrustum(xmin, xmax, ymin, ymax, nnear, ffar);
glTranslatef(0.0, 0.0, distance);
glMultMatrixf((GLfloat *) mInverse);
glMatrixMode(GL_MODELVIEW);
}
void testMatrixTranspose(void)
{
int **matrixA;
int **matrixAT;
int rowNumOfA, colNumOfA;
int rowNumOfAT, colNumOfAT;
rowNumOfA = 2;
colNumOfA = 3;
matrixA = generateMatrix(rowNumOfA, colNumOfA);
rowNumOfAT = colNumOfA;
colNumOfAT = rowNumOfA;
matrixAT = make2DArray(rowNumOfAT, colNumOfAT);
printf("----------- test: matrix transpose ------------------\n");
matrixTranspose(matrixAT, matrixA, rowNumOfA, colNumOfA);
printf("matrixA\n");
printMatrix(matrixA, rowNumOfA, colNumOfA);
printf("matrixAT\n");
printMatrix(matrixAT, rowNumOfAT, colNumOfAT);
}
void nebu_Camera_Rotate(nebu_Camera *pCamera, int flags,
float dx, float dy)
{
// adjust up vector, so that it is orthogonal to
// view direction
vec3 vDiff, vView, vRight, vUp;
vec3 vdx, vdy;
vec3 *pvCenter, *pvMoving;
switch(flags & NEBU_CAMERA_ROTATE_MASK)
{
case NEBU_CAMERA_ROTATE_AROUND_EYE:
pvCenter = &pCamera->vEye;
pvMoving = &pCamera->vLookAt;
break;
case NEBU_CAMERA_ROTATE_AROUND_LOOKAT:
pvCenter = &pCamera->vLookAt;
pvMoving = &pCamera->vEye;
break;
default:
assert(0);
return;
}
vec3_Sub(&vDiff, pvCenter, pvMoving);
vec3_Normalize(&vView, &vDiff);
vec3_Cross(&vRight, &pCamera->vUp, &vView);
vec3_Normalize(&vRight, &vRight);
vec3_Cross(&vUp, &vView, &vRight);
vec3_Normalize(&vUp, &vUp);
// horizontal movement (dx):
if(dx == 0) {
vec3_Zero(&vdx);
} else {
// rotate moving around up vector through center point
vec3 v = vDiff;
float fAngle = dx * 2 * (float)M_PI / 360.0f;
matrix matRotation;
matrixRotationAxis(&matRotation, fAngle, &vUp);
vec3_Transform(&v, &vDiff, &matRotation);
vec3_Sub(&vdx, &v, &vDiff);
}
// vertical movement (dy):
if(dy == 0) {
vec3_Zero(&vdy);
} else {
// rotate eye point around up vector through lookAt point
vec3 v = vDiff;
float fAngle = dy * 2 * (float)M_PI / 360.0f;
matrix matRotation;
matrixRotationAxis(&matRotation, fAngle, &vRight);
vec3_Transform(&v, &vDiff, &matRotation);
vec3_Sub(&vdy, &v, &vDiff);
matrixTranspose(&matRotation, &matRotation);
if(!(flags & NEBU_CAMERA_FIXED_UP))
vec3_Transform(&pCamera->vUp, &pCamera->vUp, &matRotation);
}
{
vec3 v;
vec3_Add(&v, &vdx, &vdy);
vec3_Add(&v, &v, pvMoving);
vec3_Sub(&v, &v, pvCenter);
vec3_Normalize(&v, &v);
// printf("up dot view: %.4f\n", - vec3_Dot(&v, &pCamera->vUp));
vec3_Scale(&v, &v, vec3_Length(&vDiff));
vec3_Add(pvMoving, &v, pvCenter);
}
}
// LLSQ Approx. Trilateration
void locateLLSQ(int numantennas, double* antennas, double* distances, double* x)
{
// function saves triangulated position in vector x
// input *antennas holds
// x1, y1, z1,
// x2, y2, z2,
// x3, y3, z3,
// x4, y4, z4;
// numantennas holds integer number of antennas (min 5)
// double *distances holds pointer to array holding distances from each antenna
// Theory used:
// http://inside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf
// define some locals
int m = (numantennas - 1); // Number of rows in A
int n = 3; // Number of columns in A (always three)
if (m >= 4)n = 4; // Number of columns in A: three coordinates plus K
double A[m * n];
double b[m * 1];
double r;
double Atranspose[n * m];
double AtAinA[n * m];
double AtA[n * n];
//Calculating b vector
for (int i = 0; i < m; i++)
{
r = dist(antennas[0], antennas[1], antennas[2], antennas[(i + 1) * 3], antennas[(i + 1) * 3 + 1], antennas[(i + 1) * 3 + 2]);
b[i] = 0.5*(distances[0] * distances[0] - distances[i + 1] * distances[i + 1] + r * r); // If given exact distances
if (n == 4)b[i] = 0.5 * r*r; // For the case when we calculate K, overwrite b
}
#ifdef VERBOSE
matrixPrint(b, m, 1, "b");
#endif // DEBUG
//Calculating A matrix
for (int i = 0; i < m; i++)
{
A[i * n] = antennas[(i + 1) * 3] - antennas[0]; //xi-x1
A[i * n + 1] = antennas[(i + 1) * 3 + 1] - antennas[1]; //yi-y1
A[i * n + 2] = antennas[(i + 1) * 3 + 2] - antennas[2]; //zi-z1
if (n == 4) A[i * n + 3] = -0.5 * (distances[0] * distances[0] - distances[i + 1] * distances[i + 1]); // for K
}
#ifdef VERBOSE
matrixPrint(A, m, n, "A");
#endif // DEBUG
matrixTranspose(A, m, n, Atranspose);
matrixMult(Atranspose, A, n, m, n, AtA);
if (matrixInvert(AtA, n) == 1) //well behaved
{
matrixMult(AtA, Atranspose, n, n, m, AtAinA);
matrixMult(AtAinA, b, n, m, 1, x);
}
else // Ill-behaved A
{
double Q[m * m], Qtranspose[m * m], Qtransposeb[m * 1];
double R[m * m];
QR(A, m, n, Q, R);
matrixTranspose(Q, m, m, Qtranspose);
matrixMult(Qtranspose, b, m, m, 1, Qtransposeb);
matrixInvert(R, m);
matrixMult(R, Qtransposeb, m, m, 1, x);
}
// Adding back the reference point
x[0] = x[0] + antennas[0];
x[1] = x[1] + antennas[1];
x[2] = x[2] + antennas[2];
if (n == 4)x[3] = 1 / sqrt(x[3]); // Calculate K
}
// QR Factorization
void QR(double* A, int m, int n, double* Q, double* R)
{
// Not a very efficient approach, but simple and effective
// Written by Nico van Duijn, 9/13/2015
// Source of algorithm (adjusted by me):
// http://www.keithlantz.net/2012/05/qr-decomposition-using-householder-transformations/
// A is the (m*n) matrix to be factorized A=Q*R
double mag, alpha;
double u[m], v[m], vtrans[m];
double P[m * m], I[m * m], vvtrans[m * m], Rbackup[m * m], Qbackup[m * m];
matrixCopy(A, m, m, R); // Initialize R to A
// Initialize Q, P, I to Identity
for (int i = 0; i < m * m; i++)Q[i] = 0;
for (int i = 0; i < m; i++)Q[i * n + i] = 1;
for (int i = 0; i < m * m; i++)P[i] = 0;
for (int i = 0; i < m; i++)P[i * n + i] = 1;
for (int i = 0; i < m * m; i++)I[i] = 0;
for (int i = 0; i < m; i++)I[i * n + i] = 1;
for (int i = 0; i < n; i++) //loop through all columns
{
for (int q = 0; q < m; q++)u[q] = 0; // set u and v to zero
for (int q = 0; q < m; q++)v[q] = 0;
mag = 0.0; //set mag to zero
for (int j = i; j < m; j++)
{
u[j] = R[j * n + i];
mag += u[j] * u[j];
}
mag = sqrt(mag);
alpha = u[i] < 0 ? mag : -mag;
mag = 0.0;
for (int j = i; j < m; j++)
{
v[j] = j == i ? u[j] + alpha : u[j];
mag += v[j] * v[j];
}
mag = sqrt(mag);
if (mag < 0.0000000001) continue;
for (int j = i; j < m; j++) v[j] /= mag;
// P = I - (v * v.transpose()) * 2.0;
matrixTranspose(v, m, 1, vtrans);
matrixMult(v, vtrans, m, 1, m, vvtrans);
matrixScale(vvtrans, m, m, 2.0);
matrixSub(I, vvtrans, m, m, P);
// R = P * R;
matrixMult(P, R, m, m, m, Rbackup);
matrixCopy(Rbackup, m, m, R);
//Q = Q * P;
matrixMult(Q, P, m, m, m, Qbackup);
matrixCopy(Qbackup, m, m, Q);
}
}
Matrix Matrix::matrixInvert(Matrix *matrix)
{
Matrix result;
float matrix3x3[9];
/* Find the cofactor of each element. */
/* Element (i, j) (1, 1) */
matrix3x3[0] = matrix->elements[ 5];
matrix3x3[1] = matrix->elements[ 6];
matrix3x3[2] = matrix->elements[ 7];
matrix3x3[3] = matrix->elements[ 9];
matrix3x3[4] = matrix->elements[10];
matrix3x3[5] = matrix->elements[11];
matrix3x3[6] = matrix->elements[13];
matrix3x3[7] = matrix->elements[14];
matrix3x3[8] = matrix->elements[15];
result.elements[0] = matrixDeterminant(matrix3x3);
/* Element (i, j) (1, 2) */
matrix3x3[0] = matrix->elements[ 1];
matrix3x3[1] = matrix->elements[ 2];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 9];
matrix3x3[4] = matrix->elements[10];
matrix3x3[5] = matrix->elements[11];
matrix3x3[6] = matrix->elements[13];
matrix3x3[7] = matrix->elements[14];
matrix3x3[8] = matrix->elements[15];
result.elements[4] = -matrixDeterminant(matrix3x3);
/* Element (i, j) (1, 3) */
matrix3x3[0] = matrix->elements[ 1];
matrix3x3[1] = matrix->elements[ 2];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 5];
matrix3x3[4] = matrix->elements[ 6];
matrix3x3[5] = matrix->elements[ 7];
matrix3x3[6] = matrix->elements[13];
matrix3x3[7] = matrix->elements[14];
matrix3x3[8] = matrix->elements[15];
result.elements[8] = matrixDeterminant(matrix3x3);
/* Element (i, j) (1, 4) */
matrix3x3[0] = matrix->elements[ 1];
matrix3x3[1] = matrix->elements[ 2];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 5];
matrix3x3[4] = matrix->elements[ 6];
matrix3x3[5] = matrix->elements[ 7];
matrix3x3[6] = matrix->elements[ 9];
matrix3x3[7] = matrix->elements[10];
matrix3x3[8] = matrix->elements[11];
result.elements[12] = -matrixDeterminant(matrix3x3);
/* Element (i, j) (2, 1) */
matrix3x3[0] = matrix->elements[ 4];
matrix3x3[1] = matrix->elements[ 6];
matrix3x3[2] = matrix->elements[ 7];
matrix3x3[3] = matrix->elements[ 8];
matrix3x3[4] = matrix->elements[10];
matrix3x3[5] = matrix->elements[11];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[14];
matrix3x3[8] = matrix->elements[15];
result.elements[1] = -matrixDeterminant(matrix3x3);
/* Element (i, j) (2, 2) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 2];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 8];
matrix3x3[4] = matrix->elements[10];
matrix3x3[5] = matrix->elements[11];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[14];
matrix3x3[8] = matrix->elements[15];
result.elements[5] = matrixDeterminant(matrix3x3);
/* Element (i, j) (2, 3) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 2];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 4];
matrix3x3[4] = matrix->elements[ 6];
matrix3x3[5] = matrix->elements[ 7];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[14];
matrix3x3[8] = matrix->elements[15];
result.elements[9] = -matrixDeterminant(matrix3x3);
/* Element (i, j) (2, 4) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 2];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 4];
matrix3x3[4] = matrix->elements[ 6];
matrix3x3[5] = matrix->elements[ 7];
matrix3x3[6] = matrix->elements[ 8];
matrix3x3[7] = matrix->elements[10];
matrix3x3[8] = matrix->elements[11];
result.elements[13] = matrixDeterminant(matrix3x3);
/* Element (i, j) (3, 1) */
matrix3x3[0] = matrix->elements[ 4];
matrix3x3[1] = matrix->elements[ 5];
matrix3x3[2] = matrix->elements[ 7];
matrix3x3[3] = matrix->elements[ 8];
matrix3x3[4] = matrix->elements[ 9];
matrix3x3[5] = matrix->elements[11];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[13];
matrix3x3[8] = matrix->elements[15];
result.elements[2] = matrixDeterminant(matrix3x3);
/* Element (i, j) (3, 2) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 1];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 8];
matrix3x3[4] = matrix->elements[ 9];
matrix3x3[5] = matrix->elements[11];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[13];
matrix3x3[8] = matrix->elements[15];
result.elements[6] = -matrixDeterminant(matrix3x3);
/* Element (i, j) (3, 3) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 1];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 4];
matrix3x3[4] = matrix->elements[ 5];
matrix3x3[5] = matrix->elements[ 7];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[13];
matrix3x3[8] = matrix->elements[15];
result.elements[10] = matrixDeterminant(matrix3x3);
/* Element (i, j) (3, 4) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 1];
matrix3x3[2] = matrix->elements[ 3];
matrix3x3[3] = matrix->elements[ 4];
matrix3x3[4] = matrix->elements[ 5];
matrix3x3[5] = matrix->elements[ 7];
matrix3x3[6] = matrix->elements[ 8];
matrix3x3[7] = matrix->elements[ 9];
matrix3x3[8] = matrix->elements[11];
result.elements[14] = -matrixDeterminant(matrix3x3);
/* Element (i, j) (4, 1) */
matrix3x3[0] = matrix->elements[ 4];
matrix3x3[1] = matrix->elements[ 5];
matrix3x3[2] = matrix->elements[ 6];
matrix3x3[3] = matrix->elements[ 8];
matrix3x3[4] = matrix->elements[ 9];
matrix3x3[5] = matrix->elements[10];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[13];
matrix3x3[8] = matrix->elements[14];
result.elements[3] = -matrixDeterminant(matrix3x3);
/* Element (i, j) (4, 2) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 1];
matrix3x3[2] = matrix->elements[ 2];
matrix3x3[3] = matrix->elements[ 8];
matrix3x3[4] = matrix->elements[ 9];
matrix3x3[5] = matrix->elements[10];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[13];
matrix3x3[8] = matrix->elements[14];
result.elements[7] = matrixDeterminant(matrix3x3);
/* Element (i, j) (4, 3) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 1];
matrix3x3[2] = matrix->elements[ 2];
matrix3x3[3] = matrix->elements[ 4];
matrix3x3[4] = matrix->elements[ 5];
matrix3x3[5] = matrix->elements[ 6];
matrix3x3[6] = matrix->elements[12];
matrix3x3[7] = matrix->elements[13];
matrix3x3[8] = matrix->elements[14];
result.elements[11] = -matrixDeterminant(matrix3x3);
/* Element (i, j) (4, 4) */
matrix3x3[0] = matrix->elements[ 0];
matrix3x3[1] = matrix->elements[ 1];
matrix3x3[2] = matrix->elements[ 2];
matrix3x3[3] = matrix->elements[ 4];
matrix3x3[4] = matrix->elements[ 5];
matrix3x3[5] = matrix->elements[ 6];
matrix3x3[6] = matrix->elements[ 8];
matrix3x3[7] = matrix->elements[ 9];
matrix3x3[8] = matrix->elements[10];
result.elements[15] = matrixDeterminant(matrix3x3);
/* The adjoint is the transpose of the cofactor matrix. */
matrixTranspose(&result);
/* The inverse is the adjoint divided by the determinant. */
result = matrixScale(&result, 1.0f / matrixDeterminant(matrix));
return result;
}
int main(int argc, char *argv[])
{
//initialize
int i, j, k, l, mpi_rank, mpi_size, mpi_rowsize, mpi_colsize, subN, sqrtP;
int row_rank, col_rank, row, col, destR, destC, src, srcR, srcC;
// declare variables to store time of parallelism
double execTime, execStart, execEnd;
MPI_Comm rowComm, colComm;
MPI_Init(&argc, &argv);
MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);
MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);
if (argc < 2) {
printf("error: need one argument for filename");
exit(-1);
}
// declare variables of type vector to read matrices
vector * mat1;
vector * mat2;
float * vector1;
float * vector2;
//create data on root
// read matrices
if (mpi_rank == ROOT)
{
mat1 = readfile(argv[1], N, N);
mat2 = readfile(argv[2], N, N);
vector1 = mat1->data;
vector2 = mat2->data;
}
// find the sub matrix dimention
sqrtP = (int) sqrt(mpi_size);
subN = N/sqrtP;
//allocate memory for N/4xN rows, and NxN/4 columns
float * row_mat, *col_mat, *row_mat_rec, *col_mat_rec, *col_matT;
double *can_res, *can_out;
//allocate memory for the buffers
row_mat = allocateFloatMatrix(subN, subN);
row_mat_rec = allocateFloatMatrix(subN, subN);
col_mat = allocateFloatMatrix(subN, subN);
col_matT = allocateFloatMatrix(subN, subN);
col_mat_rec = allocateFloatMatrix(subN, subN);
can_res = allocateDoubleMatrix(subN, subN);
can_out = allocateDoubleMatrix(N, N);
//create and commit datatypes
MPI_Datatype arrtype, resized_arrtype, arrtypeD, resized_arrtypeD;
int sizes[2] = { N,N };
int subsizes[2] = { subN,subN };
int starts[2] = { 0,0 };
MPI_Type_create_subarray(2, sizes, subsizes, starts, MPI_ORDER_C, MPI_FLOAT, &arrtype);
MPI_Type_create_resized(arrtype, 0, subN * sizeof(float), &resized_arrtype);
MPI_Type_commit(&resized_arrtype);
MPI_Type_create_subarray(2, sizes, subsizes, starts, MPI_ORDER_C, MPI_DOUBLE, &arrtypeD);
MPI_Type_create_resized(arrtypeD, 0, subN * sizeof(double), &resized_arrtypeD);
MPI_Type_commit(&resized_arrtypeD);
//calculate send counts and displacements
int * counts, * displs;
counts = (int *) malloc(mpi_size * sizeof(int));
displs = (int *) malloc(mpi_size * sizeof(int));
for(i = 0; i < mpi_size; i++)
{
counts[i] = 1;
displs[i] = N*(i/sqrtP) + (i%sqrtP);
}
//start timer, compute dot product and record execution time
execStart = MPI_Wtime();
//scatterv subarrays
MPI_Scatterv(vector1, counts, displs, resized_arrtype, row_mat, subN*subN, MPI_FLOAT, ROOT, MPI_COMM_WORLD);
MPI_Scatterv(vector2, counts, displs, resized_arrtype, col_mat, subN*subN, MPI_FLOAT, ROOT, MPI_COMM_WORLD);
//get row comm and rank
row = mpi_rank/sqrtP;
MPI_Comm_split(MPI_COMM_WORLD,row,mpi_rank,&rowComm);
MPI_Comm_rank(rowComm,&row_rank);
MPI_Comm_size(rowComm, &mpi_rowsize);
//get col comm and rank
col = mpi_rank%sqrtP;
MPI_Comm_split(MPI_COMM_WORLD,col,mpi_rank,&colComm);
MPI_Comm_rank(colComm,&col_rank);
MPI_Comm_size(colComm, &mpi_colsize);
//MPI_Barrier(MPI_COMM_WORLD);
// find the source and destination in row communicator - to left shift rows by row number
destR = row_rank-row;
if (destR < 0) {
destR = destR + mpi_rowsize;
}
srcR = row_rank+row;
if (srcR > (mpi_rowsize-1)) {
srcR = srcR - mpi_rowsize;
}
// find the source and destination in column communicator - to north shift columns by column number
destC = col_rank - col;
if (destC < 0) {
destC = destC + mpi_colsize;
}
srcC = col_rank+col;
if (srcC > (mpi_colsize-1)) {
srcC = srcC - mpi_colsize;
}
// left shift rows by row number
MPI_Sendrecv(row_mat, subN*subN, MPI_FLOAT, destR, 0, row_mat_rec, subN*subN, MPI_FLOAT, srcR, MPI_ANY_TAG, rowComm, MPI_STATUS_IGNORE);
//north shift columns by column number
MPI_Sendrecv(col_mat, subN*subN, MPI_FLOAT, destC, 1, col_mat_rec, subN*subN, MPI_FLOAT, srcC, MPI_ANY_TAG, colComm, MPI_STATUS_IGNORE);
for (l=0; l<sqrtP; l++)
{
memcpy(row_mat, row_mat_rec, sizeof(float)*subN*subN);
memcpy(col_mat, col_mat_rec, sizeof(float)*subN*subN);
// Finding the transpose of matrix B
matrixTranspose(subN, col_mat, col_matT);
//perform a partial matrix-vector multiplication on each process
matrixMultiplyT(subN, row_mat, col_matT, can_res);
// find the source and destination in row communicator - to left shift all rows once
if (row_rank != 0) {
destR = row_rank - 1;
} else {
destR = mpi_rowsize - 1;
}
srcR = row_rank + 1;
if (srcR == mpi_rowsize) {
srcR = 0;
}
// find the source and destination in column communicator - to north shift all columns once
if (col_rank != 0) {
destC = col_rank - 1;
} else {
destC = mpi_colsize - 1;
}
srcC = col_rank + 1;
if (srcC == mpi_colsize) {
srcC = 0;
}
//left shift all rows once
MPI_Sendrecv(row_mat, subN*subN, MPI_FLOAT, destR, 2, row_mat_rec, subN*subN, MPI_FLOAT, srcR, MPI_ANY_TAG, rowComm, MPI_STATUS_IGNORE);
//north shift all columns once
MPI_Sendrecv(col_mat, subN*subN, MPI_FLOAT, destC, 3, col_mat_rec, subN*subN, MPI_FLOAT, srcC, MPI_ANY_TAG, colComm, MPI_STATUS_IGNORE);
}
// gather the matrix multiplication results from all procs
MPI_Gatherv(can_res, subN*subN, MPI_DOUBLE, can_out, counts, displs, resized_arrtypeD, ROOT, MPI_COMM_WORLD);
//stop timer
execEnd = MPI_Wtime();
execTime = execEnd - execStart;
//free datatypes
MPI_Type_free(&resized_arrtype);
MPI_Type_free(&resized_arrtypeD);
if (mpi_rank == ROOT)
{
printf("Execution time for dot product: %f seconds\n", execTime);
printf("Result: %f, %f, %f \n ", can_out[0], can_out[2047*N + 2047], can_out[4095*N + 4095]);
free(vector1);
free(vector2);
}
free(row_mat);
free(col_mat);
free(row_mat_rec);
free(col_mat_rec);
free(col_matT);
free(can_res);
free(can_out);
//shut down MPI
MPI_Finalize();
return 0;
}
void nebu_Camera_RotateAroundTarget(nebu_Camera *pCamera,
float dx, float dy) {
// adjust up vector, so that it is orthogonal to
// view direction
vec3 vDiff, vView, vRight, vUp;
vec3 vdx, vdy;
vec3_Sub(&vDiff, &pCamera->vLookAt, &pCamera->vEye);
vec3_Normalize(&vView, &vDiff);
vec3_Cross(&vRight, &pCamera->vUp, &vView);
vec3_Normalize(&vRight, &vRight);
vec3_Cross(&vUp, &vView, &vRight);
vec3_Normalize(&vUp, &vUp);
// horizontal movement (dx):
if(dx == 0) {
vec3_Zero(&vdx);
} else {
// rotate eye point around up vector through lookAt point
vec3 v = vDiff;
float fAngle = dx * 2 * M_PI / 360.0;
matrix matRotation;
matrixRotationAxis(&matRotation, fAngle, &vUp);
vec3_Transform(&v, &vDiff, &matRotation);
vec3_Sub(&vdx, &v, &vDiff);
}
// vertical movement (dy):
if(dy == 0) {
vec3_Zero(&vdy);
} else {
// rotate eye point around up vector through lookAt point
vec3 v = vDiff;
float fAngle = dy * 2 * M_PI / 360.0;
matrix matRotation;
matrixRotationAxis(&matRotation, fAngle, &vRight);
vec3_Transform(&v, &vDiff, &matRotation);
vec3_Sub(&vdy, &v, &vDiff);
matrixTranspose(&matRotation, &matRotation);
vec3_Transform(&pCamera->vUp, &pCamera->vUp, &matRotation);
}
// add relative movements to camera position
/*
vec3_Add(&pCamera->vEye, &pCamera->vEye, &vdx);
vec3_Add(&pCamera->vEye, &pCamera->vEye, &vdy);
*/
{
vec3 v;
vec3_Add(&v, &vdx, &vdy);
vec3_Add(&v, &v, &pCamera->vEye);
vec3_Sub(&v, &v, &pCamera->vLookAt);
vec3_Normalize(&v, &v);
// printf("up dot view: %.4f\n", - vec3_Dot(&v, &pCamera->vUp));
vec3_Scale(&v, &v, vec3_Length(&vDiff));
vec3_Add(&pCamera->vEye, &v, &pCamera->vLookAt);
}
}
/*!
* This sub-routine computes the velocity vector, given a magnitude, a unit normal vector from the
* point on the surface where the regolith is lofted from, and the two conic angles which describe
* the velocity vector's direction relative to the normal vector. A backwards approach is used
* to go from the velocity vector in the final rotated intermediate frame back to the body fixed
* frame. More details are given in the thesis report and author's personal notes.
*
*/
void computeRegolithVelocityVector( std::vector< double > regolithPositionVector,
const double velocityMagnitude,
const double coneAngleAzimuth,
const double coneAngleDeclination,
std::vector< double > &unitNormalVector,
std::vector< double > ®olithVelocityVector )
{
// form the velocity vector, assuming that the intermediate frame's z-axis, on the surface of
// the asteroid, is along the final velocity vector
regolithVelocityVector[ 0 ] = 0.0;
regolithVelocityVector[ 1 ] = 0.0;
regolithVelocityVector[ 2 ] = velocityMagnitude;
// get the rotatin matrix to go from the rotated intermediate frame back to the initial
// frame where the z-axis was along the normal axis and the x-axis was pointing towards north
// direction
std::vector< std::vector< double > > zBasicRotationMatrix
{ { std::cos( coneAngleAzimuth ), std::sin( coneAngleAzimuth ), 0.0 },
{ -std::sin( coneAngleAzimuth ), std::cos( coneAngleAzimuth ), 0.0 },
{ 0.0, 0.0, 1.0 } };
std::vector< std::vector< double > > yBasicRotationMatrix
{ { std::cos( coneAngleDeclination ), 0.0, -std::sin( coneAngleDeclination ) },
{ 0.0, 1.0, 0.0 },
{ std::sin( coneAngleDeclination ), 0.0, std::cos( coneAngleDeclination ) } };
std::vector< std::vector< double > > nonTransposedRotationMatrix( 3, std::vector< double > ( 3 ) );
matrixMultiplication( yBasicRotationMatrix,
zBasicRotationMatrix,
nonTransposedRotationMatrix,
3,
3,
3,
3 );
std::vector< std::vector< double > > intermediateFrameRotationMatrix( 3, std::vector< double > ( 3 ) );
matrixTranspose( nonTransposedRotationMatrix, intermediateFrameRotationMatrix );
std::vector< std::vector< double > > rotatedIntermediateFrameVelocityVector
{ { regolithVelocityVector[ 0 ] },
{ regolithVelocityVector[ 1 ] },
{ regolithVelocityVector[ 2 ] } };
std::vector< std::vector< double > > intermediateFrameVelocityVector( 3, std::vector< double > ( 1 ) );
matrixMultiplication( intermediateFrameRotationMatrix,
rotatedIntermediateFrameVelocityVector,
intermediateFrameVelocityVector,
3,
3,
3,
1 );
// now obtain the basis vectors for the intermediate frame expressed in the
// body fixed frame coordinates
std::vector< double > xUnitVector( 3 );
std::vector< double > yUnitVector( 3 );
std::vector< double > zUnitVector( 3 );
zUnitVector = unitNormalVector;
std::vector< double > bodyFrameZUnitVector { 0.0, 0.0, 1.0 };
// get the intermediate RTN frame at the surface point
std::vector< double > unitR = normalize( regolithPositionVector );
std::vector< double > unitT = normalize( crossProduct( unitR, bodyFrameZUnitVector ) );
// get the x basis vector, pointing to north
xUnitVector = normalize( crossProduct( unitT, zUnitVector ) );
// get the y basis vector
yUnitVector = normalize( crossProduct( zUnitVector, xUnitVector ) );
std::vector< double > zPrincipalAxisBodyFrame { 0.0, 0.0, 1.0 };
std::vector< double > zNegativePrincipalAxisBodyFrame { 0.0, 0.0, -1.0 };
// check if the position vector is along the poles
const double positionDotPrincipalZ
= dotProduct( normalize( regolithPositionVector ), zPrincipalAxisBodyFrame );
const double positionDotNegativePrincipalZ
= dotProduct( normalize( regolithPositionVector ), zNegativePrincipalAxisBodyFrame );
if( positionDotPrincipalZ == 1.0 )
{
// the position vector is pointing to the poles, hence x basis vector pointing to the north
// direction wouldn't work
xUnitVector = { 1.0, 0.0, 0.0 };
yUnitVector = { 0.0, 1.0, 0.0 };
}
else if( positionDotNegativePrincipalZ == 1.0 )
{
xUnitVector = { -1.0, 0.0, 0.0 };
yUnitVector = { 0.0, 1.0, 0.0 };
}
// put the basis vectors in a 3x3 matrix
std::vector< std::vector< double > > intermediateFrameBasisMatrix
{ { xUnitVector[ 0 ], yUnitVector[ 0 ], zUnitVector[ 0 ] },
{ xUnitVector[ 1 ], yUnitVector[ 1 ], zUnitVector[ 1 ] },
{ xUnitVector[ 2 ], yUnitVector[ 2 ], zUnitVector[ 2 ] } };
std::vector< std::vector< double > > bodyFrameVelocityVector( 3, std::vector< double >( 1 ) );
matrixMultiplication( intermediateFrameBasisMatrix,
intermediateFrameVelocityVector,
bodyFrameVelocityVector,
3,
3,
3,
1 );
// return the final regolith velocity vector, expressed in body frame coordinates
regolithVelocityVector[ 0 ] = bodyFrameVelocityVector[ 0 ][ 0 ];
regolithVelocityVector[ 1 ] = bodyFrameVelocityVector[ 1 ][ 0 ];
regolithVelocityVector[ 2 ] = bodyFrameVelocityVector[ 2 ][ 0 ];
}
