int Eigen(gsl_matrix_complex *A, gsl_matrix_complex *vec, gsl_vector_complex *val) {
gsl_matrix_complex *T;
T=gsl_matrix_complex_alloc(A->size1,A->size2);
// Matrix_Copy(T,A);
Matrix_Transpose(T,A);
char jobvl='N';
char jobvr='V';
int n=T->size1;
int lda=T->tda;
int ldvl=T->tda;
int ldvr=T->tda;
int info;
int lwork=2*n;
double *rwork;
double _Complex *work;
rwork=(double*)malloc(2*n*sizeof(double));
work=(double _Complex*)malloc(sizeof(double _Complex)*lwork);
zgeev_(&jobvl,&jobvr,&n, (double _Complex*)T->data, &lda, (double _Complex*)val->data,NULL, &ldvl,(double _Complex*)vec->data, &ldvr, work, &lwork, rwork, &info);
Matrix_Transpose(vec);
// for(int k=0;k<A->size2;k++){
// double _Complex Scale;
// for(int l=0;l<n;l++)
// Scale+=cpow(cabs(matrix_get(vec,l,k)),2);
// printf("NORM %E %E\n",creal(Scale),cimag(Scale));
// for(int l=0;l<n;l++){
// matrix_div(vec,l,k,csqrt(Scale));
// }
// }
gsl_matrix_complex_free(T);
return info;
}
void LU_Refactorize(PT_Basis pB)
{
char L = 'L'; /* lower triangular */
char D = 'U'; /* unit triangular matrix (diagonals are ones) */
ptrdiff_t info, incx=1, incp;
/* Matrix_Print_row(pB->pLX); */
/* Matrix_Print_utril_row(pB->pUX); */
/* factorize using lapack */
dgetrf(&(Matrix_Rows(pB->pF)), &(Matrix_Rows(pB->pF)),
pMAT(pB->pF), &((pB->pF)->rows_alloc), pB->p, &info);
/* store upper triangular matrix (including the diagonal to Ut), i.e. copy Ut <- F */
/* lapack ignores remaining elements below diagonal when computing triangular solutions */
Matrix_Copy(pB->pF, pB->pUt, pB->w);
/* transform upper part of F (i.e. Ut) to triangular row major matrix UX*/
/* UX <- F */
Matrix_Full2RowTriangular(pB->pF, pB->pUX, pB->r);
/* invert lower triangular part */
dtrtri( &L, &D, &(Matrix_Rows(pB->pF)), pMAT(pB->pF),
&((pB->pF)->rows_alloc), &info);
/* set strictly upper triangular parts to zeros because L is a full matrix
* and we need zeros to compute proper permutation inv(L)*P */
Matrix_Uzeros(pB->pF);
/* transpose matrix because dlaswp operates rowwise and we need columnwise */
/* LX <- F' */
Matrix_Transpose(pB->pF, pB->pLX, pB->r);
/* interchange columns according to pivots in pB->p and write to LX*/
incp = -1; /* go backwards */
dlaswp( &(Matrix_Rows(pB->pLX)), pMAT(pB->pLX), &((pB->pLX)->rows_alloc),
&incx, &(Matrix_Rows(pB->pLX)) , pB->p, &incp);
/* Matrix_Print_col(pB->pX); */
/* Matrix_Print_row(pB->pLX); */
/* Matrix_Print_col(pB->pUt); */
/* Matrix_Print_utril_row(pB->pUX); */
/* matrix F after solution is factored in [L\U], we want the original format for the next call
to dgesv, thus create a copy F <- X */
Matrix_Copy(pB->pX, pB->pF, pB->w);
}
/* creates row major triangular UX matrix from a full triangular matrix F */
void Matrix_Full2RowTriangular(PT_Matrix pF, PT_Matrix pU, double *w)
{
ptrdiff_t r, c;
/* transpose U <- F' */
Matrix_Transpose(pF, pU, w);
/* put upper diagonal elements to proper position in matrix F */
for (r=0; r<pU->rows; r++) {
for (c=0; c<pU->cols; c++) {
if (c>=r) {
/* access upper triangular elements from matrix pU */
pU->A[pU->rows_alloc*r+c-(r+1)*r/2] = pF->A[r+c*pF->rows_alloc];
}
}
}
}
void CG_DrawTestBox( vec3_t origin, vec3_t mins, vec3_t maxs, vec3_t angles )
{
vec3_t start, end, vec;
float linewidth = 6;
vec3_t localAxis[3];
#if 1
vec3_t ax[3];
AnglesToAxis( angles, ax );
Matrix_Transpose( ax, localAxis );
#else
Matrix_Copy( axis_identity, localAxis );
if( angles[YAW] ) Matrix_Rotate( localAxis, -angles[YAW], 0, 0, 1 );
if( angles[PITCH] ) Matrix_Rotate( localAxis, -angles[PITCH], 0, 1, 0 );
if( angles[ROLL] ) Matrix_Rotate( localAxis, -angles[ROLL], 1, 0, 0 );
#endif
//horizontal projection
start[0] = mins[0];
start[1] = mins[1];
start[2] = mins[2];
end[0] = mins[0];
end[1] = mins[1];
end[2] = maxs[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = mins[0];
start[1] = maxs[1];
start[2] = mins[2];
end[0] = mins[0];
end[1] = maxs[1];
end[2] = maxs[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = maxs[0];
start[1] = mins[1];
start[2] = mins[2];
end[0] = maxs[0];
end[1] = mins[1];
end[2] = maxs[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = maxs[0];
start[1] = maxs[1];
start[2] = mins[2];
end[0] = maxs[0];
end[1] = maxs[1];
end[2] = maxs[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
//x projection
start[0] = mins[0];
start[1] = mins[1];
start[2] = mins[2];
end[0] = maxs[0];
end[1] = mins[1];
end[2] = mins[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = mins[0];
start[1] = maxs[1];
start[2] = maxs[2];
end[0] = maxs[0];
end[1] = maxs[1];
end[2] = maxs[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = mins[0];
start[1] = maxs[1];
start[2] = mins[2];
end[0] = maxs[0];
end[1] = maxs[1];
end[2] = mins[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = mins[0];
start[1] = mins[1];
start[2] = maxs[2];
end[0] = maxs[0];
end[1] = mins[1];
end[2] = maxs[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
//z projection
start[0] = mins[0];
start[1] = mins[1];
start[2] = mins[2];
end[0] = mins[0];
end[1] = maxs[1];
end[2] = mins[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = maxs[0];
start[1] = mins[1];
start[2] = maxs[2];
end[0] = maxs[0];
end[1] = maxs[1];
end[2] = maxs[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = maxs[0];
start[1] = mins[1];
start[2] = mins[2];
end[0] = maxs[0];
end[1] = maxs[1];
end[2] = mins[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
start[0] = mins[0];
start[1] = mins[1];
start[2] = maxs[2];
end[0] = mins[0];
end[1] = maxs[1];
end[2] = maxs[2];
// convert to local axis space
VectorCopy( start, vec );
Matrix_TransformVector( localAxis, vec, start );
VectorCopy( end, vec );
Matrix_TransformVector( localAxis, vec, end );
VectorAdd( origin, start, start );
VectorAdd( origin, end, end );
CG_QuickPolyBeam( start, end, linewidth, NULL );
}
/* headingKalman
*
* Implements a 1-dimensional, 1st order Kalman Filter
*
* That is, it deals with heading and heading rate (h and h') but no other
* state variables. The state equations are:
*
* X = A X^
* h = h + h'dt --> | h | = | 1 dt | | h |
* h' = h' | h' | | 0 1 | | h' |
*
* Kalman Filtering is not that hard. If it's hard you haven't found the right
* teacher. Try taking CS373 from Udacity.com
*
* This notation is Octave (Matlab) syntax and is based on the Bishop-Welch
* paper and references the equation numbers in that paper.
* http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
*
* returns : current heading estimate
*/
float headingKalman(float dt, float Hgps, bool gps, float dHgyro, bool gyro)
{
A[1] = dt;
/* Initialize, first time thru
x = H*z0
*/
//fprintf(stdout, "gyro? %c gps? %c\n", (gyro)?'Y':'N', (gps)?'Y':'N');
// Depending on what sensor measurements we've gotten,
// switch between observer (H) matrices and measurement noise (R) matrices
// TODO 3 incorporate HDOP or sat count in R
if (gps) {
H[0] = 1.0;
z[0] = Hgps;
} else {
H[0] = 0;
z[0] = 0;
}
if (gyro) {
H[3] = 1.0;
z[1] = dHgyro;
} else {
H[3] = 0;
z[1] = 0;
}
//Matrix_print(2,2, A, "1. A");
//Matrix_print(2,2, P, " P");
//Matrix_print(2,1, x, " x");
//Matrix_print(2,1, K, " K");
//Matrix_print(2,2, H, "2. H");
//Matrix_print(2,1, z, " z");
/**********************************************************************
* Predict
%
* In this step we "move" our state estimate according to the equation
*
* x = A*x; // Eq 1.9
***********************************************************************/
float xp[2];
Matrix_Multiply(2,2,1, xp, A, x);
//Matrix_print(2,1, xp, "3. xp");
/**********************************************************************
* We also have to "move" our uncertainty and add noise. Whenever we move,
* we lose certainty because of system noise.
*
* P = A*P*A' + Q; // Eq 1.10
***********************************************************************/
float At[4];
Matrix_Transpose(2,2, At, A);
float AP[4];
Matrix_Multiply(2,2,2, AP, A, P);
float APAt[4];
Matrix_Multiply(2,2,2, APAt, AP, At);
Matrix_Add(2,2, P, APAt, Q);
//Matrix_print(2,2, P, "4. P");
/**********************************************************************
* Measurement aka Correct
* First, we have to figure out the Kalman Gain which is basically how
* much we trust the sensor measurement versus our prediction.
*
* K = P*H'*inv(H*P*H' + R); // Eq 1.11
***********************************************************************/
float Ht[4];
//Matrix_print(2,2, H, "5. H");
Matrix_Transpose(2,2, Ht, H);
//Matrix_print(2,2, Ht, "5. Ht");
float HP[2];
//Matrix_print(2,2, P, "5. P");
Matrix_Multiply(2,2,2, HP, H, P);
//Matrix_print(2,2, HP, "5. HP");
float HPHt[4];
Matrix_Multiply(2,2,2, HPHt, HP, Ht);
//Matrix_print(2,2, HPHt, "5. HPHt");
float HPHtR[4];
//Matrix_print(2,2, R, "5. R");
Matrix_Add(2,2, HPHtR, HPHt, R);
//Matrix_print(2,2, HPHtR, "5. HPHtR");
Matrix_Inverse(2, HPHtR);
//Matrix_print(2,2, HPHtR, "5. HPHtR");
float PHt[2];
//Matrix_print(2,2, P, "5. P");
//Matrix_print(2,2, Ht, "5. Ht");
Matrix_Multiply(2,2,2, PHt, P, Ht);
//Matrix_print(2,2, PHt, "5. PHt");
Matrix_Multiply(2,2,2, K, PHt, HPHtR);
//Matrix_print(2,2, K, "5. K");
/**********************************************************************
* Then we determine the discrepancy between prediction and measurement
* with the "Innovation" or Residual: z-H*x, multiply that by the
* Kalman gain to correct the estimate towards the prediction a little
* at a time.
*
* x = x + K*(z-H*x); // Eq 1.12
***********************************************************************/
float Hx[2];
Matrix_Multiply(2,2,1, Hx, H, xp);
//Matrix_print(2,2, H, "6. H");
//Matrix_print(2,1, x, "6. x");
//Matrix_print(2,1, Hx, "6. Hx");
float zHx[2];
Matrix_Subtract(2,1, zHx, z, Hx);
zHx[0] = clamp180(zHx[0]);
//Matrix_print(2,1, z, "6. z");
//Matrix_print(2,1, zHx, "6. zHx");
float KzHx[2];
Matrix_Multiply(2,2,1, KzHx, K, zHx);
//Matrix_print(2,2, K, "6. K");
//Matrix_print(2,1, KzHx, "6. KzHx");
Matrix_Add(2,1, x, xp, KzHx);
x[0] = clamp360(x[0]); // Clamp to 0-360 range
//Matrix_print(2,1, x, "6. x");
/**********************************************************************
* We also have to adjust the certainty. With a new measurement, the
* estimate certainty always increases.
*
* P = (I-K*H)*P; // Eq 1.13
***********************************************************************/
float KH[4];
//Matrix_print(2,2, K, "7. K");
Matrix_Multiply(2,2,2, KH, K, H);
//Matrix_print(2,2, KH, "7. KH");
float IKH[4];
Matrix_Subtract(2,2, IKH, I, KH);
//Matrix_print(2,2, IKH, "7. IKH");
float P2[4];
Matrix_Multiply(2,2,2, P2, IKH, P);
Matrix_Copy(2, 2, P, P2);
//Matrix_print(2,2, P, "7. P");
return x[0];
}
// Create an object in front of the character and fire
// Parameters:
// GO: The object getting passed in, in this case the character
// State: Needed to grab members
void State_CharacterController_ShootBullet(GObject* GO, State* state)
{
//Get members
struct State_CharacterController_Members* members = (struct State_CharacterController_Members*)state->members;
// Camera local
Camera* cam = RenderingManager_GetRenderingBuffer()->camera;
// Gets the time per second
float dt = TimeManager_GetDeltaSec();
members->timer += dt;
if(InputManager_GetInputBuffer().mouseLock)
{
// Create a net movement vector
Vector direction;
Vector_INIT_ON_STACK(direction, 3);
if (InputManager_IsMouseButtonPressed(0) && members->timer >= members->coolDown)
{
//Get "forward" Vector
Matrix_SliceRow(&direction, cam->rotationMatrix, 2, 0, 3);
Vector_Scale(&direction,-1.0f);
// Create the bullet object
GObject* bullet = GObject_Allocate();
GObject_Initialize(bullet);
//bullet->mesh = AssetManager_LookupMesh("Sphere");
bullet->mesh = AssetManager_LookupMesh("Arrow");
bullet->texture = AssetManager_LookupTexture("Arrow");
bullet->body = RigidBody_Allocate();
RigidBody_Initialize(bullet->body, bullet->frameOfReference, 0.45f);
bullet->body->coefficientOfRestitution = 0.2f;
bullet->collider = Collider_Allocate();
ConvexHullCollider_Initialize(bullet->collider);
ConvexHullCollider_MakeRectangularCollider(bullet->collider->data->convexHullData, 0.1f, 2.0f, 0.1f);
//AABBCollider_Initialize(bullet->collider, 2.0f, 2.0f, 2.0f, &Vector_ZERO);
//Lay arrow flat
GObject_Rotate(bullet, &Vector_E1, -3.14159f / 2.0f);
//Construct a rotation matrix to orient bullet
Matrix rot;
Matrix_INIT_ON_STACK(rot, 3, 3);
//Grab 4,4 minor to get 3x3 rotation matrix of camera
Matrix_GetMinor(&rot, cam->rotationMatrix, 3, 3);
//Transpose it to get correct direction
Matrix_Transpose(&rot);
//Rotate the bullet
Matrix_TransformMatrix(&rot, bullet->frameOfReference->rotation);
Matrix_TransformMatrix(&rot, bullet->body->frame->rotation);
Vector vector;
Vector_INIT_ON_STACK(vector,3);
vector.components[0] = 0.9f;
vector.components[1] = 1.0f;
vector.components[2] = 0.9f;
GObject_Scale(bullet, &vector);
Vector translation;
Vector_INIT_ON_STACK(translation, 3);
Vector_GetScalarProduct(&translation, &direction, 2.82843);
GObject_Translate(bullet, GO->frameOfReference->position);
GObject_Translate(bullet, &translation);
Vector_Scale(&direction, 25.0f);
//Vector_Increment(bullet->body->velocity,&direction);
RigidBody_ApplyImpulse(bullet->body,&direction,&Vector_ZERO);
//Add remove state
State* state = State_Allocate();
State_Remove_Initialize(state, 5.0f);
GObject_AddState(bullet, state);
ObjectManager_AddObject(bullet);
members->timer = 0;
}
}
}
