void Normalize(void)
{
float error = 0;
float temporary[3][3];
float renorm = 0;
error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20
renorm = .5 * (3 - Vector_Dot_Product(&temporary[0][0], &temporary[0][0])); //eq.21
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm = .5 * (3 - Vector_Dot_Product(&temporary[1][0], &temporary[1][0])); //eq.21
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm = .5 * (3 - Vector_Dot_Product(&temporary[2][0], &temporary[2][0])); //eq.21
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}
void Normalize(void)
{
fix16_t error = 0;
fix16_t temporary[3][3];
fix16_t renorm = 0;
error = -fix16_mul(Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0]),
const_fix16_from_dbl(.5));
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3),
-Vector_Dot_Product(&temporary[0][0],&temporary[0][0])));
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3),
-Vector_Dot_Product(&temporary[1][0],&temporary[1][0])));
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm = fix16_mul(const_fix16_from_dbl(.5), fix16_sadd(fix16_from_int(3),
- Vector_Dot_Product(&temporary[2][0],&temporary[2][0])));
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(Accel_Vector[0] * Accel_Vector[0] + Accel_Vector[1]
* Accel_Vector[1] + Accel_Vector[2] * Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY_DIV; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
// Accel_weight = constrain(1 - 2 * abs(1 - Accel_magnitude), 0, 1); //
Accel_weight = 1 - 2 * fabs(1 - Accel_magnitude);
// constrain
if (Accel_weight > 1)
Accel_weight = 1;
else if (Accel_weight < 0)
Accel_weight = 0;
Vector_Cross_Product(&errorRollPitch[0], &Accel_Vector[0],
&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCHa * Accel_weight);
Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCHa
* Accel_weight);
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);
#ifdef NO_MAGNETOMETER
float mag_heading_x;
float mag_heading_y;
float errorCourse;
static float Scaled_Omega_P[3];
//*****YAW***************
// We make the gyro YAW drift correction based on compass magnetic heading
mag_heading_x = cos(MAG_Heading);
mag_heading_y = sin(MAG_Heading);
errorCourse = (DCM_Matrix[0][0] * mag_heading_y) - (DCM_Matrix[1][0]
* mag_heading_x); //Calculating YAW error
Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);//.01proportional of YAW.
Vector_Add(Omega_P, Omega_P, Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);//.00001Integrator
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);//adding integrator to the Omega_I
#endif
}
void DCM_driftCorrection(float* accelVector, float scaledAccelMag, float magneticHeading)
{
//Compensation the Roll, Pitch and Yaw drift.
float magneticHeading_X;
float magneticHeading_Y;
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_weight;
float Integrator_magnitude;
//*****Roll and Pitch***************
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - scaledAccelMag),0,1);
Vector_Cross_Product(&errorRollPitch[0],&accelVector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0], &errorRollPitch[0], DCM_RollPitch_Kp*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],DCM_RollPitch_Ki*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
// //Calculate Heading_X and Heading_Y
// magneticHeading_X = cos(magneticHeading);
// magneticHeading_Y = sin(magneticHeading);
//
// // We make the gyro YAW drift correction based on compass magnetic heading
// errorCourse=(DCM_Matrix[0][0]*magneticHeading_Y) - (DCM_Matrix[1][0]*magneticHeading_X); //Calculating YAW error
// Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
//
// Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],DCM_Yaw_Kp);
// Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
//
// Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],DCM_Yaw_Ki);
// Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
//
//
// // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
// Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
// if (Integrator_magnitude > ToRad(300)) {
// Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
// }
}
///
//Lets the runner jump off of a wall
// obj: A pointer to the object
// state: A pointer to the runner controller state which is allowing the object to wall jump
void State_RunnerController_WallJump(GObject* obj, State* state)
{
//Get the members of this state
struct State_RunnerController_Members* members = (struct State_RunnerController_Members*)state->members;
Vector impulse;
Vector_INIT_ON_STACK(impulse, 3);
Vector_Copy(&impulse, members->wallNormal);
Vector_Scale(&impulse, members->jumpMag * 2);
RigidBody_ApplyImpulse(obj->body, &impulse, &Vector_ZERO);
Vector_Copy(&impulse, &Vector_E2);
Vector_Scale(&impulse, members->jumpMag);
RigidBody_ApplyImpulse(obj->body, &impulse, &Vector_ZERO);
}
///
//Translates the RenderingManager's camera according to keyboard input
//
//Parameters:
// GO: The game object this state is attached to
// state: the First Person Camera State updating the gameObject
void State_FirstPersonCamera_Translate(GObject* GO, State* state)
{
Camera* cam = RenderingManager_GetRenderingBuffer().camera;
if(InputManager_GetInputBuffer().mouseLock)
{
Vector netMvmtVec;
Vector partialMvmtVec;
Vector_INIT_ON_STACK(netMvmtVec, 3);
Vector_INIT_ON_STACK(partialMvmtVec, 3);
if (InputManager_IsKeyDown('w'))
{
//Get "back" Vector
Matrix_SliceRow(&partialMvmtVec, cam->rotationMatrix, 2, 0, 3);
//Subtract "back" Vector from netMvmtVec
Vector_Decrement(&netMvmtVec, &partialMvmtVec);
//Or in one step but less pretty... Faster though. I think I want readable here for now though.
//Vector_DecrementArray(netMvmtVec.components, Matrix_Index(cam->rotationMatrix, 2, 0), 3);
}
if (InputManager_IsKeyDown('s'))
{
//Get "back" Vector
Matrix_SliceRow(&partialMvmtVec, cam->rotationMatrix, 2, 0, 3);
//Add "back" Vector to netMvmtVec
Vector_Increment(&netMvmtVec, &partialMvmtVec);
}
if (InputManager_IsKeyDown('a'))
{
//Get "Right" Vector
Matrix_SliceRow(&partialMvmtVec, cam->rotationMatrix, 0, 0, 3);
//Subtract "Right" Vector From netMvmtVec
Vector_Decrement(&netMvmtVec, &partialMvmtVec);
}
if (InputManager_IsKeyDown('d'))
{
//Get "Right" Vector
Matrix_SliceRow(&partialMvmtVec, cam->rotationMatrix, 0, 0, 3);
//Add "Right" Vector to netMvmtVec
Vector_Increment(&netMvmtVec, &partialMvmtVec);
}
float dt = TimeManager_GetDeltaSec();
if (Vector_GetMag(&netMvmtVec) > 0.0f && dt > 0.0f)
{
Vector_Normalize(&netMvmtVec);
Vector_Scale(&netMvmtVec, state->members->movementSpeed * dt);
Camera_Translate(cam, &netMvmtVec);
}
}
}
void Drift_correction(fix16_t head_x, fix16_t head_y)
{
fix16_t errorCourse;
static fix16_t Scaled_Omega_P[3];
static fix16_t Scaled_Omega_I[3];
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]);
errorRollPitch[0] = constrain(errorRollPitch[0],-fix16_from_int(1000),fix16_from_int(1000));
errorRollPitch[1] = constrain(errorRollPitch[1],-fix16_from_int(1000),fix16_from_int(1000));
errorRollPitch[2] = constrain(errorRollPitch[2],-fix16_from_int(1000),fix16_from_int(1000));
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
#ifdef IsMAG
errorCourse= fix16_sadd(fix16_mul(DCM_Matrix[0][0], head_x),
fix16_mul(DCM_Matrix[1][0],head_y));
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse);
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
errorYaw[0] = constrain(errorYaw[0],-fix16_from_int(50),fix16_from_int(50));
errorYaw[1] = constrain(errorYaw[1],-fix16_from_int(50),fix16_from_int(50));
errorYaw[2] = constrain(errorYaw[2],-fix16_from_int(50),fix16_from_int(50));
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
#endif
}
///
//Lets the ParkourController vault over a wall
//
//Parameters:
// obj: A pointer to the object attached to the ParkourController state
// state:
void State_ParkourController_WallVault(GObject* obj, State* state)
{
//Get the members of this state
struct State_ParkourController_Members* members = (struct State_ParkourController_Members*)state->members;
Vector impulse;
Vector_INIT_ON_STACK(impulse, 3);
Vector_Copy(&impulse, members->wallNormal);
Vector_Scale(&impulse, -members->maxVelocity);
RigidBody_ApplyImpulse(obj->body, &impulse, &Vector_ZERO);
}
///
//Lets the runner vault off of a wall
// obj: A pointer to the object
// state: A pointer to the runner controller state which is allowing the object to wall vault
void State_RunnerController_WallVault(GObject* obj, State* state)
{
//Get the members of this state
struct State_RunnerController_Members* members = (struct State_RunnerController_Members*)state->members;
Vector impulse;
Vector_INIT_ON_STACK(impulse, 3);
Vector_Copy(&impulse, members->wallNormal);
float mag = obj->body->velocity->components[1];
Vector_Scale(&impulse, -mag * 1.0f);
RigidBody_ApplyImpulse(obj->body, &impulse, &Vector_ZERO);
Vector_Copy(&impulse, &Vector_E2);
obj->body->velocity->components[1] = 0;
Vector_Scale(&impulse, members->jumpMag);
RigidBody_ApplyImpulse(obj->body, &impulse, &Vector_ZERO);
}
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
}
///
//Accelerates the runner controller
//
//PArameters:
// obj: A pointer to the game object to accelerate
// state: A pointer to rhe runner controller state which is accelerating this object
void State_RunnerController_Accelerate(GObject* obj, State* state)
{
//Grab the state members
struct State_RunnerController_Members* members = (struct State_RunnerController_Members*)state->members;
//Grab the forward vector from the camera
Camera* cam = RenderingManager_GetRenderingBuffer()->camera;
Vector forward;
Vector_INIT_ON_STACK(forward, 3);
Matrix_SliceRow(&forward, cam->rotationMatrix, 2, 0, 3);
//Project the forward vector onto the XY Plane
Vector perp;
Vector_INIT_ON_STACK(perp, 3);
Vector_GetProjection(&perp, &forward, &Vector_E2);
Vector_Decrement(&forward, &perp);
//Scale the vector to the acceleration
Vector_Normalize(&forward);
Vector_Scale(&forward, -members->acceleration);
//Only apply the impulse if the velocity is less than the max speed
if(Vector_GetMag(obj->body->velocity) - fabs(Vector_DotProduct(obj->body->velocity, &Vector_E2)) < members->maxVelocity)
{
//Apply the impulse
RigidBody_ApplyForce(obj->body, &forward, &Vector_ZERO);
}
else
{
printf("Value:\t%f\n", Vector_GetMag(obj->body->velocity) - fabs(Vector_DotProduct(obj->body->velocity, &Vector_E2)));
}
}
///
//Allows the parkour controller to run up a wall
//
//Parameters:
// obj: A pointer to the object attached to the parkourController state
// state: The parkourController state updating the object
static void State_ParkourController_Wallrun(GObject* obj, State* state)
{
//Get the members of this state
struct State_ParkourController_Members* members = (struct State_ParkourController_Members*)state->members;
//If we are not vertical wallrunning yet
if(members->verticalRunning == 0 && members->horizontalRunning == 0)
{
//Loop through the list of collisions this object was involved in
struct LinkedList_Node* currentNode = obj->collider->currentCollisions->head;
while(currentNode != NULL)
{
//Get the current collision
struct Collision* current = (struct Collision*)currentNode->data;
//Make sure this collision is with a wall
//TODO: Epsilon check!!!
if(current->minimumTranslationVector->components[0] != 0.0f || current->minimumTranslationVector->components[2] != 0.0f)
{
//Make a copy of the collision normal in case of manipulation
Vector currentNormal;
Vector_INIT_ON_STACK(currentNormal, 3);
Vector_Copy(¤tNormal, current->minimumTranslationVector);
//Make sure the normal is pointing toward this object
if(current->obj1 != obj)
{
Vector_Scale(¤tNormal, -1.0f);
}
//Next we must determine what kind of wallrun is happening
//Start by getting for forward vector of the camera
Camera* cam = RenderingManager_GetRenderingBuffer()->camera;
Vector forward;
Vector_INIT_ON_STACK(forward, 3);
Matrix_SliceRow(&forward, cam->rotationMatrix, 2, 0, 3);
//Project the forward vector onto the XY plane
Vector perp;
Vector_INIT_ON_STACK(perp, 3);
Vector_GetProjection(&perp, &forward, &Vector_E2);
Vector_Decrement(&forward, &perp);
Vector_Normalize(&forward);
//Get the dot product of the forward vector and collision normal
float dotProduct = Vector_DotProduct(&forward, ¤tNormal);
//If the dot product is closer to 1, we are starting to vertically wallrun
if(dotProduct > 0.75f)
{
members->verticalRunning = 1;
//Vertical wall running always has higher precedence than horizontal wall running
members->horizontalRunning = 0;
//SEt the wall normal of state
Vector_Copy(members->wallNormal, ¤tNormal);
break;
}
else if(dotProduct > 0.0f)
{
members->horizontalRunning = 1;
//Set the wall normal of state
Vector_Copy(members->wallNormal, ¤tNormal);
}
}
currentNode = currentNode->next;
}
}
//If we are vertical wall running
if(members->verticalRunning)
{
State_ParkourController_VerticalWallrun(obj, state);
}
//else If we are horizontal wall running
else if(members->horizontalRunning)
{
State_ParkourController_HorizontalWallrun(obj, state);
}
}
// translate the character and his bounding box.
// Parameters:
// GO: The game object this state is attached to (Used for translating the bounding box)
// state: The first person camera state updating the gameObject
void State_CharacterController_Translate(GObject* GO, State* state)
{
Camera* cam = RenderingManager_GetRenderingBuffer()->camera;
//Get members
struct State_CharacterController_Members* members = (struct State_CharacterController_Members*)state->members;
if(InputManager_GetInputBuffer().mouseLock)
{
// Gets the time per second
float dt = TimeManager_GetDeltaSec();
Vector netMvmtVec;
Vector partialMvmtVec;
Vector_INIT_ON_STACK(netMvmtVec, 3);
Vector_INIT_ON_STACK(partialMvmtVec, 3);
if (InputManager_IsKeyDown('w'))
{
//Get "back" Vector
Matrix_SliceRow(&partialMvmtVec, cam->rotationMatrix, 2, 0, 3);
//Subtract "back" Vector from netMvmtVec
Vector_Decrement(&netMvmtVec, &partialMvmtVec);
//Or in one step but less pretty... Faster though. I think I want readable here for now though.
//Vector_DecrementArray(netMvmtVec.components, Matrix_Index(cam->rotationMatrix, 2, 0), 3);
}
if (InputManager_IsKeyDown('s'))
{
//Get "back" Vector
Matrix_SliceRow(&partialMvmtVec, cam->rotationMatrix, 2, 0, 3);
//Add "back" Vector to netMvmtVec
Vector_Increment(&netMvmtVec, &partialMvmtVec);
}
if (InputManager_IsKeyDown('a'))
{
//Get "Right" Vector
Matrix_SliceRow(&partialMvmtVec, cam->rotationMatrix, 0, 0, 3);
//Subtract "Right" Vector From netMvmtVec
Vector_Decrement(&netMvmtVec, &partialMvmtVec);
}
if (InputManager_IsKeyDown('d'))
{
//Get "Right" Vector
Matrix_SliceRow(&partialMvmtVec, cam->rotationMatrix, 0, 0, 3);
//Add "Right" Vector to netMvmtVec
Vector_Increment(&netMvmtVec, &partialMvmtVec);
}
if (Vector_GetMag(&netMvmtVec) > 0.0f)
{
// Get the projection and keep player grounded
Vector perpMvmtVec;
Vector_INIT_ON_STACK(perpMvmtVec, 3);
Vector_GetProjection(&perpMvmtVec, &netMvmtVec, &Vector_E2);
Vector_Decrement(&netMvmtVec, &perpMvmtVec);
// Normalize vector and scale
Vector_Normalize(&netMvmtVec);
Vector_Scale(&netMvmtVec, members->movementSpeed);
//Apply Impulse
RigidBody_ApplyImpulse(GO->body, &netMvmtVec, &Vector_ZERO);
}
}
// If vector is going too fast, the maxspeed will keep it from going faster, by scaling it by maxspeed.
if(Vector_GetMag(GO->body->velocity) >= members->maxSpeed)
{
Vector_Normalize(GO->body->velocity);
Vector_Scale(GO->body->velocity,members->maxSpeed);
}
// Set position of Camera to the body
Camera_SetPosition(cam,GO->body->frame->position);
}
///
//Allows the runner controller to wallrun if necessary conditions are met
//
//Parameters:
// obj: A pointer to the object which is running on walls
// state: A pointer to the runner controller state which is allowing the object to wallrun
void State_RunnerController_Wallrun(GObject* obj, State* state)
{
//Get the members of this state
struct State_RunnerController_Members* members = (struct State_RunnerController_Members*)state->members;
//Get the first collision this object is involved in
Collision* first = (Collision*)obj->collider->currentCollisions->head->data;
//If we are not wallrunning yet
if(members->horizontalRunning == 0 && members->verticalRunning == 0)
{
//Make sure this is a wall
if(first->minimumTranslationVector->components[0] != 0.0 || first->minimumTranslationVector->components[2] != 0.0f)
{
//Save the normal
Vector_Copy(members->wallNormal, first->minimumTranslationVector);
if(first->obj1 != obj)
{
Vector_Scale(members->wallNormal, -1.0f);
}
//Determine what kind of wallrun is occurring
//First get the forward vector of the camera
Camera* cam = RenderingManager_GetRenderingBuffer()->camera;
Vector forward;
Vector_INIT_ON_STACK(forward, 3);
Matrix_SliceRow(&forward, cam->rotationMatrix, 2, 0, 3);
//Project the forward vector onto the XY Plane
Vector perp;
Vector_INIT_ON_STACK(perp, 3);
Vector_GetProjection(&perp, &forward, &Vector_E2);
Vector_Decrement(&forward, &perp);
Vector_Normalize(&forward);
//Get dot product of forward vector and collision normal
float dotProd = fabs(Vector_DotProduct(&forward, first->minimumTranslationVector));
//If the dot product is closer to 0 we are horizontal running, else we are vertical running
if(dotProd < 0.75)
{
members->horizontalRunning = 1;
}
else
{
members->verticalRunning = 1;
}
}
}
//If we are horizontal running
if(members->horizontalRunning == 1)
{
printf("Horizontal Wallrunnin\n");
//combat the force of gravity
Vector antiGravity;
Vector_INIT_ON_STACK(antiGravity, 3);
antiGravity.components[1] = 9.81f;
RigidBody_ApplyForce(obj->body, &antiGravity, &Vector_ZERO);
//Zero downward velocity
if(obj->body->velocity->components[1] < 0.0f)
{
Vector_Copy(&antiGravity, &Vector_ZERO);
antiGravity.components[1] = -obj->body->velocity->components[1];
RigidBody_ApplyImpulse(obj->body, &antiGravity, &Vector_ZERO);
}
State_RunnerController_Accelerate(obj, state);
}
else if(members->verticalRunning == 1)
{
printf("Vertical Wallrunnin\n");
//combat the force of gravity
Vector antiGravity;
Vector_INIT_ON_STACK(antiGravity, 3);
Vector_Copy(&antiGravity, &Vector_E2);
//go up!
Vector_Scale(&antiGravity, 9.81);
RigidBody_ApplyForce(obj->body, &antiGravity, &Vector_ZERO);
//If we aren't jumping too fast yet
if(Vector_DotProduct(obj->body->velocity, &Vector_E2) < members->maxVelocity)
{
Vector_Copy(&antiGravity, &Vector_E2);
Vector_Scale(&antiGravity, members->acceleration);
RigidBody_ApplyForce(obj->body, &antiGravity, &Vector_ZERO);
}
}
}
void Drift_correction()
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
float Integrator_magnitude;
// Local Working Variables
float errorRollPitch[3];
float errorYaw[3];
float errorCourse;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(accel_float.x * accel_float.x + accel_float.y * accel_float.y + accel_float.z * accel_float.z);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = Chop(1 - 2 * fabs(1 - Accel_magnitude), 0, 1); //
#if PERFORMANCE_REPORTING == 1
{
//amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
float tempfloat = ((Accel_weight - 0.5) * 256.0f);
imu_health += tempfloat;
Bound(imu_health, 129, 65405);
}
#endif
Vector_Cross_Product(&errorRollPitch[0], &accel_float.x, &DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0], &errorRollPitch[0], Kp_ROLLPITCH * Accel_weight);
Vector_Scale(&Scaled_Omega_I[0], &errorRollPitch[0], Ki_ROLLPITCH * Accel_weight);
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I);
//*****YAW***************
#if USE_MAGNETOMETER
// We make the gyro YAW drift correction based on compass magnetic heading
// float mag_heading_x = cos(MAG_Heading);
// float mag_heading_y = sin(MAG_Heading);
// 2D dot product
//Calculating YAW error
errorCourse = (DCM_Matrix[0][0] * MAG_Heading_Y) + (DCM_Matrix[1][0] * MAG_Heading_X);
//Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
#else // Use GPS Ground course to correct yaw gyro drift
if (ahrs_dcm.gps_course_valid) {
float course = ahrs_dcm.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
float COGX = cosf(course); //Course overground X axis
float COGY = sinf(course); //Course overground Y axis
errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
//Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW);
Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
}
#if USE_MAGNETOMETER_ONGROUND == 1
PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
else if (launch == FALSE) {
float COGX = mag_float.x; // Non-Tilt-Compensated (for filter stability reasons)
float COGY = mag_float.y; // Non-Tilt-Compensated (for filter stability reasons)
errorCourse = (DCM_Matrix[0][0] * COGY) - (DCM_Matrix[1][0] * COGX); //Calculating YAW error
//Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(errorYaw, &DCM_Matrix[2][0], errorCourse);
// P only
Vector_Scale(&Scaled_Omega_P[0], &errorYaw[0], Kp_YAW / 10.0);
Vector_Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding Proportional.fi
}
#endif // USE_MAGNETOMETER_ONGROUND
#endif
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I, Omega_I));
if (Integrator_magnitude > RadOfDeg(300)) {
Vector_Scale(Omega_I, Omega_I, 0.5f * RadOfDeg(300) / Integrator_magnitude);
}
}
void Normalize(void)
{
float error = 0;
float temporary[3][3];
float renorm = 0;
uint8_t problem = FALSE;
// Find the non-orthogonality of X wrt Y
error = -Vector_Dot_Product(&DCM_Matrix[0][0], &DCM_Matrix[1][0]) * .5; //eq.19
// Add half the XY error to X, and half to Y
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
// The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
Vector_Cross_Product(&temporary[2][0], &temporary[0][0], &temporary[1][0]); // c= a x b //eq.20
// Normalize lenght of X
renorm = Vector_Dot_Product(&temporary[0][0], &temporary[0][0]);
// a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
// b) if the norm is further from 1, use a real sqrt
// c) norm is huge: disaster! reset! mayday!
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
// Normalize lenght of Y
renorm = Vector_Dot_Product(&temporary[1][0], &temporary[1][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
// Normalize lenght of Z
renorm = Vector_Dot_Product(&temporary[2][0], &temporary[2][0]);
if (renorm < 1.5625f && renorm > 0.64f) {
renorm = .5 * (3 - renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm = 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
}
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
// Reset on trouble
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
set_dcm_matrix_from_rmat(orientationGetRMat_f(&ahrs_dcm.body_to_imu));
problem = FALSE;
}
}
void Vector_Normalize(fVector_t V, fVector_t Vout)
{
float magn = Vector_Magnitude(V);
Vector_Scale(Vout, V, 1.0 / magn);
}
void Vector_Normalize(Vector_t V)
{
float magn = Vector_Magnitude(V);
Vector_Scale(V, V, 1.0 / magn);
}
// 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;
}
}
}
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
float Integrator_magnitude;
// Local Working Variables
float errorRollPitch[3];
float errorYaw[3];
float errorCourse;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
#if USE_HIGH_ACCEL_FLAG
// Test for high acceleration:
// - low speed
// - high thrust
if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
if (high_accel_flag) {
Accel_weight = 0.;
}
#endif
#if PERFORMANCE_REPORTING == 1
{
float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
imu_health += tempfloat;
Bound(imu_health,129,65405);
}
#endif
Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
#if USE_MAGNETOMETER
// We make the gyro YAW drift correction based on compass magnetic heading
// float mag_heading_x = cos(MAG_Heading);
// float mag_heading_y = sin(MAG_Heading);
// 2D dot product
errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
#else // Use GPS Ground course to correct yaw gyro drift
if (ahrs_impl.gps_course_valid) {
float course = ahrs_impl.gps_course - M_PI; //This is the runaway direction of you "plane" in rad
float COGX = cosf(course); //Course overground X axis
float COGY = sinf(course); //Course overground Y axis
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}
#endif
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > RadOfDeg(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*RadOfDeg(300)/Integrator_magnitude);
}
}
void State_ParkourController_Shoot(GObject* obj, State* state)
{
//Get the members of the state
struct State_ParkourController_Members* members = (struct State_ParkourController_Members*)state->members;
//Get a reference to the camera
Camera* cam = RenderingManager_GetRenderingBuffer()->camera;
if(InputManager_GetInputBuffer().mouseLock)
{
//IF we can shoot again
if(members->shootTimer >= members->shootCooldown)
{
//Get the forward vector of the camera
Vector direction;
Vector_INIT_ON_STACK(direction, 3);
Matrix_SliceRow(&direction, cam->rotationMatrix, 2, 0, 3);
Vector_Scale(&direction, -1.0f);
//Create the bullet object
GObject* bullet = GObject_Allocate();
GObject_Initialize(bullet);
//Set the appearance
bullet->mesh = AssetManager_LookupMesh("Cube");
//bullet->texture = AssetManager_LookupTexture("White");
bullet->material = Material_Allocate();
Material_Initialize(bullet->material, AssetManager_LookupTexture("Jacob"));
//*Matrix_Index(bullet->material->colorMatrix, 1, 1) = 0.0f;
//*Matrix_Index(bullet->material->colorMatrix, 2, 2) = 0.0f;
//Create ridgid body
bullet->body = RigidBody_Allocate();
RigidBody_Initialize(bullet->body, bullet->frameOfReference, 1.0f);
bullet->body->coefficientOfRestitution = 0.2f;
bullet->body->rollingResistance = 0.2f;
bullet->body->staticFriction = 0.4f;
bullet->body->dynamicFriction = 0.2f;
//Create collider
bullet->collider = Collider_Allocate();
ConvexHullCollider_Initialize(bullet->collider);
ConvexHullCollider_MakeRectangularCollider(bullet->collider->data->convexHullData, 2.0f, 2.0f, 2.0f);
//Position bullet
Vector transform;
Vector_INIT_ON_STACK(transform, 3);
Vector_GetScalarProduct(&transform, &direction, 2.8243f);
Vector_Increment(&transform, obj->frameOfReference->position);
GObject_Translate(bullet, &transform);
Vector_Copy(&transform, &Vector_ZERO);
transform.components[2] = 1.0f;
GObject_Rotate(bullet, &transform, 3.14159f);
//Scale bullet
Vector_Copy(&transform, &Vector_ZERO);
transform.components[0] = transform.components[1] = transform.components[2] = 0.5f;
GObject_Scale(bullet, &transform);
//Apply impulse
Vector_Scale(&direction, 25.0f);
RigidBody_ApplyImpulse(bullet->body, &direction, &Vector_ZERO);
//Add the remove state
State* state = State_Allocate();
State_Remove_Initialize(state, 7.0f);
GObject_AddState(bullet, state);
//Add the bullet to the world
ObjectManager_AddObject(bullet);
//Set shoot timer to 0
members->shootTimer = 0.0f;
}
}
}
bool Matrix_ComputeInverse(const Matrix *src,Matrix * inv)
{
uint r,c,i,dim;
Matrix * m;
assert( src && inv );
assert( src->dimension == inv->dimension );
// matrix inverse by Gauss-Jordan elimination
m = Matrix_CreateDuplicate(src);
Matrix_SetIdentity(inv);
dim = m->dimension;
for(c=0;c<dim;c++)
{
uint maxR;
double val,maxVal;
Vector * swap;
// try to make the cth column look the identity
// first, pivot the large value into row c:
maxVal = m->rows[c]->element[c];
maxR = c;
for(r=c+1;r<dim;r++)
{
val = m->rows[r]->element[c];
if ( val > maxVal )
{
maxVal = val;
maxR = i;
}
}
r = maxR;
if ( c != r )
{
swap = m->rows[c];
m->rows[c] = m->rows[r];
m->rows[r] = swap;
swap = inv->rows[c];
inv->rows[c] = inv->rows[r];
inv->rows[r] = swap;
}
val = m->rows[c]->element[c];
if ( ABS(val) < EPSILON )
goto fail; // !!
// subtract this row from all other rows to make their
// coefficients zero.
for(r=0;r<dim;r++)
{
double cur;
if ( r == c )
continue;
cur = m->rows[r]->element[c];
if ( ABS(cur) < EPSILON )
{
m->rows[r]->element[c] = 0.0;
}
else
{
cur = - cur/val; // the scale to make the rows[r]->column[c] zero
Vector_AddScaled(inv->rows[r],inv->rows[r], cur, inv->rows[c]);
// all the elements before c should be 0 or 1 already
m->rows[r]->element[c] = 0.0;
for(i=c+1;i<dim;i++)
{
m->rows[r]->element[i] += cur * m->rows[c]->element[i];
}
}
}
// finally, scale my row so my coefficient is 1
val = 1.0/val;
Vector_Scale(m->rows[c], val);
Vector_Scale(inv->rows[c], val);
}
Matrix_Destroy(m);
return true;
fail:
Matrix_Destroy(m);
return false;
}
///
//Allows the parkour controller to horizontally wallrun
//
//Parameters:
// obj: A pointer to the object attached to the parkourController state
// state: The parkourController state updating the object
static void State_ParkourController_HorizontalWallrun(GObject* obj, State* state)
{
printf("Horizontal\n");
//Get the members of this state
struct State_ParkourController_Members* members = (struct State_ParkourController_Members*)state->members;
//Zero downward velocity
if(obj->body->velocity->components[1] < 0.0f)
{
Vector impulse;
Vector_INIT_ON_STACK(impulse, 3);
impulse.components[1] = -obj->body->velocity->components[1];
RigidBody_ApplyImpulse(obj->body, &impulse, &Vector_ZERO);
}
//Accelerate along wall
//Get the projection of the forward vector onto the wall's plane
//Start by getting for forward vector of the camera
Camera* cam = RenderingManager_GetRenderingBuffer()->camera;
Vector forward;
Vector_INIT_ON_STACK(forward, 3);
Matrix_SliceRow(&forward, cam->rotationMatrix, 2, 0, 3);
//Forward of camera is back of object...
Vector_Scale(&forward, -1.0f);
//Project the forward vector onto the wall plane
Vector perp;
Vector_INIT_ON_STACK(perp, 3);
Vector_GetProjection(&perp, &forward, members->wallNormal);
Vector_Decrement(&forward, &perp);
//Set the Y component to 0 to get horizontal vector along wall!
forward.components[1] = 0.0f;
Vector_Normalize(&forward);
//MAke sure the velocity in this direction is not too much
float magVelAlongWall = Vector_DotProduct(&forward, obj->body->velocity);
if(magVelAlongWall < members->maxVelocity)
{
RigidBody_ApplyImpulse(obj->body, &forward, &Vector_ZERO);
}
//Apply a cohesive force to the wall to make sure you do not fall off
float mag = Vector_DotProduct(obj->body->velocity, members->wallNormal);
Vector cohesive;
Vector_INIT_ON_STACK(cohesive, 3);
if(mag > FLT_EPSILON)
{
Vector_GetScalarProduct(&cohesive, members->wallNormal, -mag);
}
else if(mag < FLT_EPSILON)
{
Vector_GetScalarProduct(&cohesive, members->wallNormal, mag);
}
RigidBody_ApplyImpulse(obj->body, &cohesive, members->wallNormal);
}
///
//Accelerates a gameobject to the degree defined by the state
//
//PArameters:
// obj: The gameobject to accelerate
// state: The ParkourController state attached to the game object
static void State_ParkourController_Accelerate(GObject* obj, State* state)
{
//Grab the state members
struct State_ParkourController_Members* members = (struct State_ParkourController_Members*)state->members;
//Grab the camera
Camera* cam = RenderingManager_GetRenderingBuffer()->camera;
//Determine the direction of acceleration
Vector netForce;
Vector_INIT_ON_STACK(netForce, 3);
Vector direction;
Vector_INIT_ON_STACK(direction, 3);
if(InputManager_IsKeyDown('w'))
{
//Get forward vector of camera
Matrix_SliceRow(&direction, cam->rotationMatrix, 2, 0, 3);
//Project onto XY Plane
Vector perp;
Vector_INIT_ON_STACK(perp, 3);
Vector_GetProjection(&perp, &direction, &Vector_E2);
Vector_Decrement(&direction, &perp);
//Add vector to netForce
//Since this is the cameras "forward vector" we must add the negative of it.
//BEcause forward is the negative Z axis
Vector_Decrement(&netForce, &direction);
}
if(InputManager_IsKeyDown('s'))
{
//Get back vector of camera
//Get forward vector of camera
Matrix_SliceRow(&direction, cam->rotationMatrix, 2, 0, 3);
//Project onto XY Plane
Vector perp;
Vector_INIT_ON_STACK(perp, 3);
Vector_GetProjection(&perp, &direction, &Vector_E2);
Vector_Decrement(&direction, &perp);
//Add vector to netForce
//Since this is the cameras "forward vector" we must add the negative of it.
//BEcause forward is the negative Z axis
Vector_Increment(&netForce, &direction);
}
if(InputManager_IsKeyDown('d'))
{
//Get forward vector of camera
Matrix_SliceRow(&direction, cam->rotationMatrix, 0, 0, 3);
//Project onto XY Plane
Vector perp;
Vector_INIT_ON_STACK(perp, 3);
Vector_GetProjection(&perp, &direction, &Vector_E2);
Vector_Decrement(&direction, &perp);
//Add vector to netForce
//Since this is the cameras "forward vector" we must add the negative of it.
//BEcause forward is the negative Z axis
Vector_Increment(&netForce, &direction);
}
if(InputManager_IsKeyDown('a'))
{
//Get forward vector of camera
Matrix_SliceRow(&direction, cam->rotationMatrix, 0, 0, 3);
//Project onto XY Plane
Vector perp;
Vector_INIT_ON_STACK(perp, 3);
Vector_GetProjection(&perp, &direction, &Vector_E2);
Vector_Decrement(&direction, &perp);
//Add vector to netForce
//Since this is the cameras "forward vector" we must add the negative of it.
//BEcause forward is the negative Z axis
Vector_Decrement(&netForce, &direction);
}
//Scale netforce to the acceleration magnitude
Vector_Normalize(&netForce);
Vector_Scale(&netForce, members->acceleration);
//Only apply impulse if velocity is less than max speed
if(Vector_GetMag(obj->body->velocity) < members->maxVelocity)
{
//Apply the impulse
RigidBody_ApplyForce(obj->body, &netForce, &Vector_ZERO);
}
else
{
//Limit velocity
Vector_Normalize(obj->body->velocity);
Vector_Scale(obj->body->velocity, members->maxVelocity);
}
}
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
float Integrator_magnitude;
// Local Working Variables
float errorRollPitch[3];
float errorYaw[3];
float errorCourse;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
#if PERFORMANCE_REPORTING == 1
{
float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
imu_health += tempfloat;
Bound(imu_health,129,65405);
}
#endif
Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
#ifdef USE_MAGNETOMETER
// We make the gyro YAW drift correction based on compass magnetic heading
// float mag_heading_x = cos(MAG_Heading);
// float mag_heading_y = sin(MAG_Heading);
// 2D dot product
errorCourse=(DCM_Matrix[0][0]*MAG_Heading_Y) + (DCM_Matrix[1][0]*MAG_Heading_X); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
#elif defined USE_GPS // Use GPS Ground course to correct yaw gyro drift
if(gps.fix == GPS_FIX_3D && gps.gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s
float ground_course = ((float)gps.course)/1.e7 - M_PI; //This is the runaway direction of you "plane" in rad
float COGX = cosf(ground_course); //Course overground X axis
float COGY = sinf(ground_course); //Course overground Y axis
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}
#endif
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > DegOfRad(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*DegOfRad(300)/Integrator_magnitude);
}
}
void DCM_normalize(void)
{
float error=0;
float temporary[3][3];
float renorm=0;
boolean problem=0;
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
if (renorm < 1.5625f && renorm > 0.64f)
{
renorm= .5 * (3-renorm); //eq.21
}
else if (renorm < 100.0f && renorm > 0.01f)
{
renorm= 1. / sqrt(renorm);
}
else
{
problem = 1;
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
if (renorm < 1.5625f && renorm > 0.64f)
{
renorm= .5 * (3-renorm); //eq.21
}
else if (renorm < 100.0f && renorm > 0.01f)
{
renorm= 1. / sqrt(renorm);
}
else
{
problem = 1;
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
if (renorm < 1.5625f && renorm > 0.64f)
{
renorm= .5 * (3-renorm); //eq.21
}
else if (renorm < 100.0f && renorm > 0.01f)
{
renorm= 1. / sqrt(renorm);
}
else
{
problem = 1;
}
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
DCM_Matrix[0][0]= 1.0f;
DCM_Matrix[0][1]= 0.0f;
DCM_Matrix[0][2]= 0.0f;
DCM_Matrix[1][0]= 0.0f;
DCM_Matrix[1][1]= 1.0f;
DCM_Matrix[1][2]= 0.0f;
DCM_Matrix[2][0]= 0.0f;
DCM_Matrix[2][1]= 0.0f;
DCM_Matrix[2][2]= 1.0f;
problem = 0;
}
}
/**************************************************************************
* \brief Filter DCM Drift Correction
*
* \param ---
*
* \return ---
***************************************************************************/
static void filter_dcm_drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
//float mag_heading_x;
//float mag_heading_y;
//static float Scaled_Omega_P[3];
//float Integrator_magnitude;
//float tempfloat;
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;
//***** Roll and Pitch ***************
/* Calculate the magnitude of the accelerometer vector */
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
//Accel_magnitude = Accel_magnitude/4096; // Scale to gravity.
/* Dynamic weighting of accelerometer info (reliability filter)
* Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) */
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],filterdata->Kp_rollPitch*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],filterdata->Ki_rollPitch*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
/*********** YAW ***************/
//#if FILTER_USE_MAGNETOMETER==1
//// We make the gyro YAW drift correction based on compass magnetic heading
//#if BOARD_VERSION < 3
//errorCourse=(DCM_Matrix[0][0]*APM_Compass.Heading_Y) - (DCM_Matrix[1][0]*APM_Compass.Heading_X); //Calculating YAW error
//#endif
//#if BOARD_VERSION == 3
//errorCourse=(DCM_Matrix[0][0]*Heading_Y) - (DCM_Matrix[1][0]*Heading_X); //Calculating YAW error
//#endif
//Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
//
//Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
//Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
//
//Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
//Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
//#else // Use GPS Ground course to correct yaw gyro drift
//if(GPS.ground_speed>=SPEEDFILT*100) // Ground speed from GPS is in m/s
//{
//COGX = cos(ToRad(GPS.ground_course/100.0));
//COGY = sin(ToRad(GPS.ground_course/100.0));
//errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
//Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
//
//Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
//Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
//
//Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
//Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
//}
//#endif
//// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
//Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
//if (Integrator_magnitude > ToRad(300)) {
//Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
//}
}
