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); //} }