void Box3D::UpdateTransformMatrix()
{
m_TransformMatrix.Identity();
if ( m_TranslateMode == modeNone )
{
return;
}
else if ( m_TranslateMode == modeMove )
{
m_TransformMatrix.SetTranslation( m_vTranslation );
return;
}
else if ( m_TranslateMode == modeScale )
{
Vector vScale( 1,1,1);
Vector vMove(0,0,0);
Vector vSize = bmaxs-bmins;
for ( int i=0; i<3; i++ )
{
float handle = m_TranslateHandle[i];
if ( vSize[i] == 0 )
continue;
if ( handle > 0 )
{
vScale[i] = (m_vTranslation[i]+vSize[i]) / vSize[i];
vMove[i] = m_vTranslation[i] / 2;
}
else if ( handle < 0 )
{
vScale[i] = (-m_vTranslation[i]+vSize[i]) / vSize[i];
vMove[i] = m_vTranslation[i] / 2;
}
}
m_TransformMatrix = m_TransformMatrix.Scale( vScale );
m_TransformMatrix.SetTranslation( vMove );
}
else if ( m_TranslateMode == modeShear )
{
Vector vSize = bmaxs-bmins;
int axisS = -1; // shear axis that wont change
int axisA = -1; // first shear axis
int axisB = -1; // second shear axis
for ( int i=0; i<3; i++ )
{
float handle = m_TranslateHandle[i];
if ( handle > 0 )
{
Assert( axisS == -1);
axisS = i;
}
else if ( handle < 0 )
{
Assert( axisS == -1);
axisS = i;
vSize *= -1;
}
else
{
if ( axisA == -1 )
axisA = i;
else
axisB = i;
}
}
Assert( (axisA!=-1) && (axisB!=-1) && (axisS!=-1) );
m_TransformMatrix.m[axisA][axisS] = (m_vTranslation[axisA])/(vSize[axisS]);
m_TransformMatrix.m[axisB][axisS] = (m_vTranslation[axisB])/(vSize[axisS]);
}
else if ( m_TranslateMode == modeRotate )
{
QAngle angle = *(QAngle*)&m_vTranslation; // buuuhhh
m_TransformMatrix.SetupMatrixOrgAngles( vec3_origin, angle );
}
// apply m_vTranslationFixPoint offset
Vector offset;
m_TransformMatrix.V3Mul( m_vTranslationFixPoint, offset );
offset = m_vTranslationFixPoint - offset;
m_TransformMatrix.m[0][3] += offset[0];
m_TransformMatrix.m[1][3] += offset[1];
m_TransformMatrix.m[2][3] += offset[2];
}
//////////////////////////////////////////////////////////////////////////
// NOTE: This function must be thread-safe. Before adding stuff contact MarcoC.
void CVehicleMovementTank::ProcessAI(const float deltaTime)
{
FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );
float dt = max( deltaTime, 0.005f);
m_movementAction.brake = false;
m_movementAction.rotateYaw = 0.0f;
m_movementAction.power = 0.0f;
float inputSpeed = 0.0f;
{
if (m_aiRequest.HasDesiredSpeed())
inputSpeed = m_aiRequest.GetDesiredSpeed();
Limit(inputSpeed, -m_maxSpeed, m_maxSpeed);
}
Vec3 vMove(ZERO);
{
if (m_aiRequest.HasMoveTarget())
vMove = ( m_aiRequest.GetMoveTarget() - m_PhysPos.pos ).GetNormalizedSafe();
}
//start calculation
if ( fabsf( inputSpeed ) < 0.0001f || m_tireBlownTimer > 1.5f )
{
// only the case to use a hand break
m_movementAction.brake = true;
}
else
{
Matrix33 entRotMatInvert( m_PhysPos.q );
entRotMatInvert.Invert();
float currentAngleSpeed = RAD2DEG(-m_PhysDyn.w.z);
const static float maxSteer = RAD2DEG(gf_PI/4.f); // fix maxsteer, shouldn't change
Vec3 vVel = m_PhysDyn.v;
Vec3 vVelR = entRotMatInvert * vVel;
float currentSpeed =vVel.GetLength();
vVelR.NormalizeSafe();
if ( vVelR.Dot( FORWARD_DIRECTION ) < 0 )
currentSpeed *= -1.0f;
// calculate pedal
static float accScale = 0.5f;
m_movementAction.power = (inputSpeed - currentSpeed) * accScale;
Limit( m_movementAction.power, -1.0f, 1.0f);
// calculate angles
Vec3 vMoveR = entRotMatInvert * vMove;
Vec3 vFwd = FORWARD_DIRECTION;
vMoveR.z =0.0f;
vMoveR.NormalizeSafe();
if ( inputSpeed < 0.0 ) // when going back
{
vFwd *= -1.0f;
vMoveR *= -1.0f;
currentAngleSpeed *=-1.0f;
}
float cosAngle = vFwd.Dot(vMoveR);
float angle = RAD2DEG( acos_tpl(cosAngle));
if ( vMoveR.Dot( Vec3( 1.0f,0.0f,0.0f ) )< 0 )
angle = -angle;
// int step =0;
m_movementAction.rotateYaw = angle * 1.75f/ maxSteer;
// implementation 1. if there is enough angle speed, we don't need to steer more
if ( fabsf(currentAngleSpeed) > fabsf(angle) && angle*currentAngleSpeed > 0.0f )
{
m_movementAction.rotateYaw = m_currSteer*0.995f;
// step =1;
}
// implementation 2. if we can guess we reach the distination angle soon, start counter steer.
float predictDelta = inputSpeed < 0.0f ? 0.1f : 0.07f;
float dict = angle + predictDelta * ( angle - m_prevAngle) / dt ;
if ( dict*currentAngleSpeed<0.0f )
{
if ( fabsf( angle ) < 2.0f )
{
m_movementAction.rotateYaw = angle* 1.75f/ maxSteer;
// step =3;
}
else
{
m_movementAction.rotateYaw = currentAngleSpeed < 0.0f ? 1.0f : -1.0f;
// step =2;
}
}
// implementation 3. tank can rotate at a point
if ( fabs( angle )> 20.0f )
{
m_movementAction.power *=0.1f;
// step =4;
}
// char buf[1024];
// sprintf(buf, "steering %4.2f %4.2f %4.2f %d\n", deltaTime,currentAngleSpeed, angle - m_prevAngle, step);
// OutputDebugString( buf );
m_prevAngle = angle;
}
}
//////////////////////////////////////////////////////////////////////////
// NOTE: This function must be thread-safe. Before adding stuff contact MarcoC.
void CVehicleMovementStdBoat::ProcessAI(const float deltaTime)
{
FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );
float dt = max( deltaTime, 0.005f);
SVehiclePhysicsStatus* physStatus = &m_physStatus[k_physicsThread];
// It is copyed from CVehicleMoventTank::ProcessAI
m_movementAction.brake = false;
m_movementAction.rotateYaw = 0.0f;
m_movementAction.power = 0.0f;
float inputSpeed = 0.0f;
{
if (m_aiRequest.HasDesiredSpeed())
inputSpeed = m_aiRequest.GetDesiredSpeed();
Limit(inputSpeed, -(m_maxSpeed*m_factorMaxSpeed), m_maxSpeed*m_factorMaxSpeed);
}
Vec3 vMove(ZERO);
{
if (m_aiRequest.HasMoveTarget())
vMove = ( m_aiRequest.GetMoveTarget() - m_pEntity->GetWorldPos() ).GetNormalizedSafe();
}
//start calculation
if ( fabsf( inputSpeed ) < 0.0001f )
{
m_movementAction.brake = true;
if ( physStatus->v.GetLength() > 1.0f )
{
m_movementAction.power =-1.0f;
}
}
else
{
Matrix33 entRotMatInvert( physStatus->q );
entRotMatInvert.Invert();
float currentAngleSpeed = RAD2DEG(-physStatus->w.z);
const static float maxSteer = RAD2DEG(gf_PI/4.f); // fix maxsteer, shouldn't change
Vec3 vVel = physStatus->v;
Vec3 vVelR = entRotMatInvert * vVel;
float currentSpeed =vVel.GetLength();
vVelR.NormalizeSafe();
if ( vVelR.Dot( FORWARD_DIRECTION ) < 0 )
currentSpeed *= -1.0f;
// calculate pedal
static float accScale = 0.5f;
m_movementAction.power = (inputSpeed - currentSpeed) * accScale;
Limit( m_movementAction.power, -1.0f, 1.0f);
// calculate angles
Vec3 vMoveR = entRotMatInvert * vMove;
Vec3 vFwd = FORWARD_DIRECTION;
vMoveR.z =0.0f;
vMoveR.NormalizeSafe();
if ( inputSpeed < 0.0f ) // when going back
{
vFwd *= -1.0f;
vMoveR *= -1.0f;
currentAngleSpeed *=-1.0f;
}
float cosAngle = vFwd.Dot(vMoveR);
float angle = RAD2DEG(acos_tpl(cosAngle));
if ( vMoveR.Dot( Vec3( 1.0f,0.0f,0.0f ) )< 0 )
angle = -angle;
//int step =0;
m_movementAction.rotateYaw = angle * 1.75f/ maxSteer;
// implementation 1. if there is enough angle speed, we don't need to steer more
if ( fabsf(currentAngleSpeed) > fabsf(angle) && angle*currentAngleSpeed > 0.0f )
{
m_movementAction.rotateYaw = m_movementAction.rotateYaw*0.995f; //step =1;
}
// implementation 2. if we can guess we reach the distination angle soon, start counter steer.
float predictDelta = inputSpeed < 0.0f ? 0.1f : 0.1f;
float dict = angle + predictDelta * ( angle - m_prevAngle) / dt ;
if ( dict*currentAngleSpeed<0.0f )
{
if ( fabsf( angle ) < 2.0f )
{
m_movementAction.rotateYaw = angle* 1.75f/ maxSteer;// ; step =3;
}
else
{
m_movementAction.rotateYaw = currentAngleSpeed < 0.0f ? 1.0f : -1.0f;// step =2;
}
}
if ( fabsf( angle ) > 20.0f && currentSpeed > 3.0f )
{
m_movementAction.power = 0.1f ;
//step =4;
}
m_prevAngle = angle;
//CryLog("steering %4.2f %4.2f %4.2f %4.2f %4.2f %4.2f %d", deltaTime,inputSpeed - currentSpeed,angle,currentAngleSpeed, m_movementAction.rotateYaw,currentAngleSpeed-m_prevAngle,step);
}
}
