void CBPGameMovement::FullWalkMove( )
{
if (player->GetGroundEntity() != NULL)
{
mv->m_vecVelocity[2] = 0.0;
Friction();
}
CheckVelocity();
GetBPPlayer()->m_flTurnRate = 0;
if (player->GetGroundEntity() != NULL && !(mv->m_nButtons & IN_JUMP))
{
WalkMove();
}
else
{
FlyMove();
}
engine->Con_NPrintf( 0, " L: %.2f, R: %.2f", bpmv->m_flLTrigger, bpmv->m_flRTrigger );
engine->Con_NPrintf( 1, "SL: %.2f, SR: %.2f", m_flLTriggerAvg, m_flRTriggerAvg );
// Set final flags.
CategorizePosition();
}
void Ball::Move()
{
vx += ax;
vy += ay;
x += vx;
y += vy;
if (abs(vx) > SPEEDMAX) vx *= SPEEDMAX / abs(vx) * 0.9;
if (abs(vy) > SPEEDMAX) vy *= SPEEDMAX / abs(vy) * 0.9;
Friction();
if (y < 0) vy = abs(vy);
else if (y > BH) vy = -abs(vy);
if (x < 0) vx = abs(vx);
else if (x > BW) vx = -abs(vx);
}
//-----------------------------------------------------------------------------
//
//-----------------------------------------------------------------------------
void UnitBaseAirLocomotion::Move( float interval, UnitBaseMoveCommand &move_command )
{
VPROF_BUDGET( "UnitBaseAirLocomotion::Move", VPROF_BUDGETGROUP_UNITS );
mv = &move_command;
mv->interval = interval;
mv->outwishvel.Init();
Friction();
FullAirMove();
MoveFacing();
// Sometimes an unit spawns with an invalid velocity and can't move
// Validate velocity after each move
CheckVelocity();
}
void CollisionRespone::run(){
unsigned int i;
findNeighbour();
std::list<compositePhysobj *>::iterator it;
for(it=parent->objects.begin();it!=parent->objects.end();it++){
if((*it)->hasRigid){
for(i=0;i<(*it)->CollsionContacts.size();i++){
DataCarrier=(*it)->CollsionContacts[i];
ImpulseResponse(DataCarrier.obj1,DataCarrier.obj2);
Friction(DataCarrier.obj1,DataCarrier.obj2);
}
}
}
/*
for(i=0;i<collisionGroups.size();i++){
ActiveCollisionList=collisionGroups[i];
ContactForceResponse();
}
*/
}
static float RankAngle (float a)
{
vec3_t wishdir;
float delta;
vec3_t ang1, ang2;
vec3_t newvel;
wishdir[0] = cos(a / 180 * M_PI);
wishdir[1] = sin(a / 180 * M_PI);
wishdir[2] = 0;
VectorCopy (ra_curvel, newvel);
Friction (newvel);
Accelerate (wishdir, movevars.maxspeed, newvel);
vectoangles (newvel, ang1);
vectoangles (ra_intentions, ang2);
delta = AngleMod(ang2[YAW] - ang1[YAW]);
return -fabs(delta);
}
int FrictionInterface(char *fname)
{
static int un = 1, nddl = NDOF;
static int n, nbis;
static int q, qdot, F;
/* Define minls=1, maxlhs, minrhs, maxrhs */
static int minlhs = 1, minrhs = 2, maxlhs = 1, maxrhs = 2;
/* Check rhs and lhs */
CheckRhs(minrhs, maxrhs) ;
CheckLhs(minlhs, maxlhs) ;
GetRhsVar(1, "d", &n, &nbis, &q);
if (n * nbis != NDOF)
{
sciprint("Wrong size!\r\n");
Error(999);
return 0;
}
GetRhsVar(2, "d", &n, &nbis, &qdot);
if (n * nbis != NDOF)
{
sciprint("Wrong size!\r\n");
Error(999);
return 0;
}
CreateVar(3, "d", &nddl, &un, &F);
Friction(stk(q), stk(qdot), stk(F));
LhsVar(1) = 3;
return 0;
}
SICONOS_EXPORT void controlLaw(double * t, double * q, double * qdot, int * NDOF, int * NCONT, double * torques)
{
int ndof;
int contacts;
char lagrangianModelName[20] = "";
getLagrangianModelName(lagrangianModelName);
getActiveDOFsize(&ndof);
vector<int> activeDOF(ndof);
if (ndof > 0)
getActiveDOF(&(activeDOF[0]));
else
ndof = *NDOF;
///////////////////////////////////////////
// Computation of the Lagrangian model
///////////////////////////////////////////
matrix<double, column_major> M(*NDOF, *NDOF);
vector<double> N(*NDOF);
if (strcmp(lagrangianModelName, "Human36") == 0)
{
double L[31];
double addl[84];
double mass;
GetModelMass(&mass);
GetAnatomicalLengths(L);
GetTag2JointLengths(addl);
InertiaH36(&(M(0, 0)), q, L, addl, mass);
NLEffectsH36(&(N[0]), q, qdot, L, addl, mass);
}
else
{
Inertia(&(M(0, 0)), q);
NLEffects(&(N[0]), q, qdot);
}
//////////////////////////////////////////////////
//Additionnal forces
/////////////////////////////////////////////////
vector<double> fAdditionnal(*NDOF);
if (strcmp(lagrangianModelName, "PA10") == 0)
Friction(&(fAdditionnal[0]), q, qdot);
else if (strcmp(lagrangianModelName, "RX90") == 0)
SpringForce(&(fAdditionnal[0]), q);
else
fAdditionnal.clear();
///////////////////////////////////////////////////
// Limitation to the selected degrees of freedom
///////////////////////////////////////////////////
reduceMatrix(M, activeDOF, activeDOF);
N = reduceVector(N, activeDOF);
fAdditionnal = reduceVector(fAdditionnal, activeDOF);
//////////////////////////////////////////////////
// Proportionnel-derivee in the task space
//////////////////////////////////////////////////
vector<double> sDesired(*NDOF);
vector<double> sdotDesired(*NDOF);
matrix<double, column_major> sddotDesiredMATRIX(*NDOF, 1);
matrix_column<matrix<double, column_major> > sddotDesired(sddotDesiredMATRIX, 1);
//vector<double> sddotDesired(*NDOF);
trajectory(t, &(sDesired[0]), &(sdotDesired[0]), &(sddotDesired[0]), &contacts);
//v = Kp*(s_desiree-TaskFunction(q))+Kv*(sdot_desiree-H*qdot)-h+sddot_desiree;
vector<double> TF(*NDOF);
vector<double> h(*NDOF);
matrix<double, column_major> H(*NDOF, *NDOF);
TaskFunction(&(TF[0]), q);
TaskJacobian(&(H(0, 0)), q);
TaskNLEffects(&(h[0]), q, qdot);
double Kp = 100.0;
double Kv = 30.0;
vector<double, array_adaptor<double> > tmp_cast(*NDOF, array_adaptor<double> (*NDOF, qdot));
sddotDesired += Kp * (sDesired - TF) + Kv * (sdotDesired - prod(H, tmp_cast)) - h;
/////////////////////////////////////////////////////////////
// Generalized Torques to make the realisation of the command
/////////////////////////////////////////////////////////////
//qddot = inv(H)*v;
vector<int> ipiv(*NDOF);
// lapack::getrf(H, ipiv);
// sddotDesired = prod(H, sddotDesired);
lapack::gesv(H, ipiv, sddotDesiredMATRIX);
//D = M*qddot(ActiveDOF) + N + FAdditionnal;
sddotDesired = reduceVector(sddotDesired, activeDOF);
N += prod(M, sddotDesired) + fAdditionnal;
//nmot = sum(ActiveDOF<=size(q, 1)); en fait ici nmot = ndof
//Torques = zeros(q);
//Torques(ActiveDOF(1:nmot)) = D(1:nmot);
for (int i = 0; i < *NDOF; i++)
torques[i] = 0;
expandVector(torques, N, activeDOF);
}
void CMomentumGameMovement::FullWalkMove()
{
if (!CheckWater())
{
StartGravity();
}
// If we are leaping out of the water, just update the counters.
if (player->m_flWaterJumpTime)
{
WaterJump();
TryPlayerMove();
// See if we are still in water?
CheckWater();
return;
}
// If we are swimming in the water, see if we are nudging against a place we can jump up out
// of, and, if so, start out jump. Otherwise, if we are not moving up, then reset jump timer to 0
if (player->GetWaterLevel() >= WL_Waist)
{
if (player->GetWaterLevel() == WL_Waist)
{
CheckWaterJump();
}
// If we are falling again, then we must not trying to jump out of water any more.
if (mv->m_vecVelocity[2] < 0 &&
player->m_flWaterJumpTime)
{
player->m_flWaterJumpTime = 0;
}
// Was jump button pressed?
if (mv->m_nButtons & IN_JUMP)
{
CheckJumpButton();
}
else
{
mv->m_nOldButtons &= ~IN_JUMP;
}
// Perform regular water movement
WaterMove();
// Redetermine position vars
CategorizePosition();
// If we are on ground, no downward velocity.
if (player->GetGroundEntity() != NULL)
{
mv->m_vecVelocity[2] = 0;
}
}
else
// Not fully underwater
{
// Was jump button pressed?
if (mv->m_nButtons & IN_JUMP)
{
CheckJumpButton();
}
else
{
mv->m_nOldButtons &= ~IN_JUMP;
}
// Fricion is handled before we add in any base velocity. That way, if we are on a conveyor,
// we don't slow when standing still, relative to the conveyor.
if (player->GetGroundEntity() != NULL)
{
mv->m_vecVelocity[2] = 0.0;
Friction();
}
// Make sure velocity is valid.
CheckVelocity();
// By default assume we did the reflect for WalkMove()
flReflectNormal = 1.0f;
if (player->GetGroundEntity() != NULL)
{
WalkMove();
}
else
{
AirMove(); // Take into account movement when in air.
}
// Set final flags.
CategorizePosition(flReflectNormal);
// Make sure velocity is valid.
CheckVelocity();
// Add any remaining gravitational component.
if (!CheckWater())
{
FinishGravity();
}
// If we are on ground, no downward velocity.
if (player->GetGroundEntity() != NULL)
{
mv->m_vecVelocity[2] = 0;
}
CheckFalling();
}
if ((m_nOldWaterLevel == WL_NotInWater && player->GetWaterLevel() != WL_NotInWater) ||
(m_nOldWaterLevel != WL_NotInWater && player->GetWaterLevel() == WL_NotInWater))
{
PlaySwimSound();
#if !defined( CLIENT_DLL )
player->Splash();
#endif
}
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CTFGameMovement::FullWalkMove()
{
if ( !InWater() )
{
StartGravity();
}
// If we are leaping out of the water, just update the counters.
if ( player->m_flWaterJumpTime )
{
// Try to jump out of the water (and check to see if we still are).
WaterJump();
TryPlayerMove();
CheckWater();
return;
}
// If we are swimming in the water, see if we are nudging against a place we can jump up out
// of, and, if so, start out jump. Otherwise, if we are not moving up, then reset jump timer to 0
if ( InWater() )
{
FullWalkMoveUnderwater();
return;
}
if (mv->m_nButtons & IN_JUMP)
{
CheckJumpButton();
}
else
{
mv->m_nOldButtons &= ~IN_JUMP;
}
// Make sure velocity is valid.
CheckVelocity();
if (player->GetGroundEntity() != NULL)
{
mv->m_vecVelocity[2] = 0.0;
Friction();
WalkMove();
}
else
{
AirMove();
}
// Set final flags.
CategorizePosition();
// Add any remaining gravitational component if we are not in water.
if ( !InWater() )
{
FinishGravity();
}
// If we are on ground, no downward velocity.
if ( player->GetGroundEntity() != NULL )
{
mv->m_vecVelocity[2] = 0;
}
// Handling falling.
CheckFalling();
// Make sure velocity is valid.
CheckVelocity();
}
void Physics::UpdateForce(Physics* forceProducer, int force_type, bool relativity)//0 magnetic, 1 contact force, 2 electric force, 3 friction
{
//vars for physics computations.
double magneticForce = 0;
double frictionForce = 0;
double electricForceX = 0;
double electricForceY = 0;
int signX = 0;
int signY = 0;
double tempForce = 0;
double degrees = math_CalculateDirectionDegrees(forceProducer->GetLoc().X, forceProducer->GetLoc().Y);
if(relativity == true && !unmovable)
{
//Calculate Newtonian forces with relativity applied
if(force_type == 3)
{
if(GetForceCount('x') < 0)
{
frictionForce = 1 * Friction(forceProducer->GetMU(), relativity) * Relativity('x');
}
else
{
frictionForce = -1 * Friction(forceProducer->GetMU(), relativity) * Relativity('x');
}
}
if(force_type == 2)
{
tempForce = math_CalculateForceFromChargedParticles(forceProducer->GetCharge(), forceProducer->GetLoc());
signX = math_Sign(forceProducer, true);
signY = math_Sign(forceProducer, false);
electricForceX = tempForce * cos(degrees) * signX;
electricForceY = tempForce * sin(degrees) * signY;
}
if(force_type == 1)
{
if(forceProducer->NewtonianForce('x') != 0)
{
ForceCountX += forceProducer->rel_NewtonianForce('x');
}
if(forceProducer->NewtonianForce('y') != 0)
{
ForceCountY += forceProducer->rel_NewtonianForce('y');
}
}
if(force_type == 0)
{
magneticForce = math_CalculateForceFromMagneticField(forceProducer->GetB2DDirection(), forceProducer->GetBMagnitude());
}
ForceCountX += electricForceX + frictionForce;
ForceCountY += electricForceY + magneticForce + (-1 * g * mass);
}
else if(!unmovable)
{
//Calculate Newtonian forces without relativity applied
if(force_type == 3)
{
if(GetForceCount('x') < 0)
{
frictionForce = 1 * Friction(forceProducer->GetMU(), relativity);
}
else
{
frictionForce = -1 * Friction(forceProducer->GetMU(), relativity);
}
}
if(force_type == 2)
{
tempForce = math_CalculateForceFromChargedParticles(forceProducer->GetCharge(), forceProducer->GetLoc());
signX = math_Sign(forceProducer, true);
signY = math_Sign(forceProducer, false);
electricForceX = tempForce * cos(degrees) * signX;
electricForceY = tempForce * sin(degrees) * signY;
}
if(force_type == 1)
{
if(forceProducer->NewtonianForce('x') != 0)
{
ForceCountX += forceProducer->NewtonianForce('x');
}
if(forceProducer->NewtonianForce('y') != 0)
{
ForceCountY += forceProducer->NewtonianForce('y');
}
}
if(force_type == 0)
{
magneticForce = math_CalculateForceFromMagneticField(forceProducer->GetB2DDirection(), forceProducer->GetBMagnitude());
}
ForceCountX += electricForceX + frictionForce;
ForceCountY += electricForceY + magneticForce + (-1 * g * mass);
}
}
