void Confo_Back::Construct_Mol_II(XYZ pre,XYZ cur,XYZ nxt,double bend,double tort,double dist,XYZ &out)
{
XYZ xyz;
double x[3],y[3];
double z[3];
double cb1[3],cb2[3];
xyz=nxt-cur;
xyz.xyz2double(x);
xyz=cur-pre;
xyz.xyz2double(y);
//check
double angle=Vector_Angle(x,y,3);
if(fabs(angle)<1.e-3||fabs(angle-M_PI)<1.e-3)
{
x[0]+=0.05;
x[2]-=0.05;
y[0]-=0.05;
y[2]+=0.05;
}
//calc
cross(z,x,y);
Universal_Rotation(z,bend,Confo_Back_ROT_TOT);
Vector_Multiply(cb1,Confo_Back_ROT_TOT,x);
Universal_Rotation(x,tort,Confo_Back_ROT_TOT);
Vector_Multiply(cb2,Confo_Back_ROT_TOT,cb1);
Vector_Normalize(cb2,3);
Vector_Dot(cb2,dist,cb2,3);
out.double2xyz(cb2);
out+=nxt;
}
//given CA -> return CB (Levitt's method)
void Confo_Beta::Confo_Beta_Levit_To_CACB(XYZ *CA,XYZ *CB,int moln,double *dist)
{
int i,j;
XYZ xyz;
double beta,theta;
//process
theta=Confo_Beta_Levit_Angle;
for(i=1;i<moln-1;i++) //omit head and tail
{
//init
if(dist==NULL)beta=Confo_Beta_Levit_Radii;
else beta=dist[i];
if(beta<0.0)
{
CB[i]=CA[i];
continue;
}
//calc
xyz=(CA[i]-CA[i-1]);
xyz.xyz2double(beta_v1);
xyz=(CA[i]-CA[i+1]);
xyz.xyz2double(beta_v2);
Vector_Addition(beta_x1,beta_v1,beta_v2,3);
Vector_Normalize(beta_x1,3);
cross(beta_x2,beta_v1,beta_v2);
Vector_Normalize(beta_x2,3);
//evaluate
CA[i].xyz2double(beta_ca);
for(j=0;j<3;j++)beta_cb[j]=beta_ca[j]+beta*(cos(theta)*beta_x1[j]+sin(theta)*beta_x2[j]);
CB[i].double2xyz(beta_cb);
}
//assign head_tail //this might be modified in the future...
if(moln>1)
{
CB[0]=CB[1]-CA[1]+CA[0];
CB[moln-1]=CB[moln-2]-CA[moln-2]+CA[moln-1];
}
else
{
CB[0]=CA[0];
CB[moln-1]=CA[moln-1];
}
}
///
//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 ARX_SPECIAL_ATTRACTORS_ComputeForIO(INTERACTIVE_OBJ * ioo, EERIE_3D * force)
{
force->x = 0;
force->y = 0;
force->z = 0;
for (long i = 0; i < MAX_ATTRACTORS; i++)
{
if (attractors[i].ionum != -1)
{
if (ValidIONum(attractors[i].ionum))
{
INTERACTIVE_OBJ * io = inter.iobj[attractors[i].ionum];
if ((io->show == SHOW_FLAG_IN_SCENE)
&& !(io->ioflags & IO_NO_COLLISIONS)
&& (io->GameFlags & GFLAG_ISINTREATZONE))
{
float power = attractors[i].power;
EERIE_3D pos;
pos.x = ioo->pos.x;
pos.y = ioo->pos.y;
pos.z = ioo->pos.z;
float dist = EEDistance3D(&pos, &io->pos);
if ((dist > ioo->physics.cyl.radius + io->physics.cyl.radius + 10.f)
|| (power < 0.f))
{
float max_radius = attractors[i].radius;
if (dist < max_radius)
{
float ratio_dist = 1.f - (dist / max_radius);
EERIE_3D vect;
vect.x = io->pos.x - pos.x;
vect.y = io->pos.y - pos.y;
vect.z = io->pos.z - pos.z;
Vector_Normalize(&vect);
power *= ratio_dist * 0.01f;
force->x = vect.x * power;
force->y = vect.y * power;
force->z = vect.z * power;
}
}
}
}
}
}
}
//
// A spring dampening function
// for the camera
//
void SpringDamp(
Vector currPos,
Vector trgPos, // Target Position
Vector prevTrgPos, // Previous Target Position
Vector result,
float springConst, // Hooke's Constant
float dampConst, // Damp Constant
float springLen)
{
Vector disp; // Displacement
Vector velocity; // Velocity
Vector mx;
Vector z;
float len;
float dot;
float forceMag; // Force Magnitude
// Calculate Spring Force
Vector_Minus(currPos, trgPos, disp);
Vector_Minus(prevTrgPos, trgPos, velocity);
len = Vector_Length(disp);
// get dot product
dot = DotProduct(disp, velocity);
forceMag = springConst * (springLen - len) +
dampConst * (dot / len);
Vector_Normalize(disp, z);
//disp *= forceMag;
Vector_Multiply(z, mx, forceMag);
printf("%0.2f %0.2f\n", mx[0], currPos[0]);
Vector_Add(currPos, mx, result);
//result[0] = currPos[0];
//result[1] = currPos[1];
//result[2] = currPos[2];
} // end of the function
void Confo_Back::Construct_CB(XYZ N,XYZ Ca,XYZ C,double bend,double tort,double dist,XYZ &Cb)
{
XYZ xyz;
double x[3],y[3];
double z[3];
double cb1[3],cb2[3];
xyz=C-Ca;
xyz.xyz2double(x);
xyz=Ca-N;
xyz.xyz2double(y);
cross(z,x,y);
Universal_Rotation(z,-1.0*bend,Confo_Back_ROT_TOT);
Vector_Multiply(cb1,Confo_Back_ROT_TOT,x);
Universal_Rotation(x,-1.0*tort,Confo_Back_ROT_TOT);
Vector_Multiply(cb2,Confo_Back_ROT_TOT,cb1);
Vector_Normalize(cb2,3);
Vector_Dot(cb2,dist,cb2,3);
Cb.double2xyz(cb2);
Cb+=Ca;
}
//===================== Recon_Beta ==================//
//given CA+CB, and AMI_Dist, update SC
void Confo_Beta::Recon_Beta_1(XYZ *CA,XYZ *CB,int moln,char *ami) //return SC
{
int i;
XYZ xyz;
double radii;
double multi;
double v[3];
for(i=0;i<moln;i++)
{
xyz=(CB[i]-CA[i]);
xyz.xyz2double(v);
radii=dot(v,v);
if(radii<1.e-3)continue;
Vector_Normalize(v,3);
multi=CB_AMI_Dist[ami[i]-'A'];
if(multi<0.0)continue;
Vector_Dot(v,multi,v,3);
xyz.double2xyz(v);
CB[i]=CA[i]+xyz;
}
}
///
//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 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);
}
}
}
///
//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);
}
}
///
//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);
}
}
/* GaussSimplify --
Given Mat1, a matrix of equalities, performs Gaussian elimination.
Find a minimum basis, Returns the rank.
Mat1 is context, Mat2 is reduced in context of Mat1
*/
int GaussSimplify(Matrix *Mat1,Matrix *Mat2) {
int NbRows = Mat1->NbRows;
int NbCols = Mat1->NbColumns;
int *column_index;
int i, j, k, n, t, pivot, Rank;
Value gcd, tmp, *cp;
column_index=(int *)malloc(NbCols * sizeof(int));
if (!column_index) {
errormsg1("GaussSimplify", "outofmem", "out of memory space\n");
Pol_status = 1;
return 0;
}
/* Initialize all the 'Value' variables */
value_init(gcd); value_init(tmp);
Rank=0;
for (j=0; j<NbCols; j++) { /* for each column starting at */
for (i=Rank; i<NbRows; i++) /* diagonal, look down to find */
if (value_notzero_p(Mat1->p[i][j])) /* the first non-zero entry */
break;
if (i!=NbRows) { /* was one found ? */
if (i!=Rank) /* was it found below the diagonal?*/
Vector_Exchange(Mat1->p[Rank],Mat1->p[i],NbCols);
/* Normalize the pivot row */
Vector_Gcd(Mat1->p[Rank],NbCols,&gcd);
/* If (gcd >= 2) */
value_set_si(tmp,2);
if (value_ge(gcd,tmp)) {
cp = Mat1->p[Rank];
for (k=0; k<NbCols; k++,cp++)
value_division(*cp,*cp,gcd);
}
if (value_neg_p(Mat1->p[Rank][j])) {
cp = Mat1->p[Rank];
for (k=0; k<NbCols; k++,cp++)
value_oppose(*cp,*cp);
}
/* End of normalize */
pivot=i;
for (i=0;i<NbRows;i++) /* Zero out the rest of the column */
if (i!=Rank) {
if (value_notzero_p(Mat1->p[i][j])) {
Value a, a1, a2, a1abs, a2abs;
value_init(a); value_init(a1); value_init(a2);
value_init(a1abs); value_init(a2abs);
value_assign(a1,Mat1->p[i][j]);
value_absolute(a1abs,a1);
value_assign(a2,Mat1->p[Rank][j]);
value_absolute(a2abs,a2);
value_gcd(a, a1abs, a2abs);
value_divexact(a1, a1, a);
value_divexact(a2, a2, a);
value_oppose(a1,a1);
Vector_Combine(Mat1->p[i],Mat1->p[Rank],Mat1->p[i],a2,
a1,NbCols);
Vector_Normalize(Mat1->p[i],NbCols);
value_clear(a); value_clear(a1); value_clear(a2);
value_clear(a1abs); value_clear(a2abs);
}
}
column_index[Rank]=j;
Rank++;
}
} /* end of Gauss elimination */
if (Mat2) { /* Mat2 is a transformation matrix (i,j->f(i,j))....
can't scale it because can't scale both sides of -> */
/* normalizes an affine transformation */
/* priority of forms */
/* 1. i' -> i (identity) */
/* 2. i' -> i + constant (uniform) */
/* 3. i' -> constant (broadcast) */
/* 4. i' -> j (permutation) */
/* 5. i' -> j + constant ( ) */
/* 6. i' -> i + j + constant (non-uniform) */
for (k=0; k<Rank; k++) {
j = column_index[k];
for (i=0; i<(Mat2->NbRows-1);i++) { /* all but the last row 0...0 1 */
if ((i!=j) && value_notzero_p(Mat2->p[i][j])) {
/* Remove dependency of i' on j */
Value a, a1, a1abs, a2, a2abs;
value_init(a); value_init(a1); value_init(a2);
value_init(a1abs); value_init(a2abs);
value_assign(a1,Mat2->p[i][j]);
value_absolute(a1abs,a1);
value_assign(a2,Mat1->p[k][j]);
value_absolute(a2abs,a2);
value_gcd(a, a1abs, a2abs);
value_divexact(a1, a1, a);
value_divexact(a2, a2, a);
value_oppose(a1,a1);
if (value_one_p(a2)) {
Vector_Combine(Mat2->p[i],Mat1->p[k],Mat2->p[i],a2,
a1,NbCols);
/* Vector_Normalize(Mat2->p[i],NbCols); -- can't do T */
} /* otherwise, can't do it without mult lhs prod (2i,3j->...) */
value_clear(a); value_clear(a1); value_clear(a2);
value_clear(a1abs); value_clear(a2abs);
}
else if ((i==j) && value_zero_p(Mat2->p[i][j])) {
/* 'i' does not depend on j */
for (n=j+1; n < (NbCols-1); n++) {
if (value_notzero_p(Mat2->p[i][n])) { /* i' depends on some n */
value_set_si(tmp,1);
Vector_Combine(Mat2->p[i],Mat1->p[k],Mat2->p[i],tmp,
tmp,NbCols);
break;
} /* if 'i' depends on just a constant, then leave it alone.*/
}
}
}
}
/* Check last row of transformation Mat2 */
for (j=0; j<(NbCols-1); j++)
if (value_notzero_p(Mat2->p[Mat2->NbRows-1][j])) {
errormsg1("GaussSimplify", "corrtrans", "Corrupted transformation\n");
break;
}
if (value_notone_p(Mat2->p[Mat2->NbRows-1][NbCols-1])) {
errormsg1("GaussSimplify", "corrtrans", "Corrupted transformation\n");
}
}
value_clear(gcd); value_clear(tmp);
free(column_index);
return Rank;
} /* GaussSimplify */
// 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);
}
