/**--------------------------------------------------------------------
;[BODY INVERSE KINEMATICS]
;BodyRotX - Global Input pitch of the body
;BodyRotY - Global Input rotation of the body
;BodyRotZ - Global Input roll of the body
;RotationY - Input Rotation for the gait
;PosX - Input position of the feet X
;PosZ - Input position of the feet Z
;BodyOffsetX - Input Offset betweeen the body and Coxa X
;BodyOffsetZ - Input Offset betweeen the body and Coxa Z
;SinB - Sin buffer for BodyRotX
;CosB - Cos buffer for BodyRotX
;SinG - Sin buffer for BodyRotZ
;CosG - Cos buffer for BodyRotZ
;BodyIKPosX - Output Position X of feet with Rotation
;BodyIKPosY - Output Position Y of feet with Rotation
;BodyIKPosZ - Output Position Z of feet with Rotation */
void BodyIK(signed int PosX, signed int PosZ, signed int PosY, signed int BodyOffsetX, signed int BodyOffsetZ, signed int RotationY) {
//Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ;
TotalX = BodyOffsetX+PosX;
//PosY are equal to a "TotalY"
//Successive global rotation matrix:
//Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
//Sinus Alfa = sinA, cosinus Alfa = cosA. and so on...
//First calculate sinus and cosinus for each rotation:
GetSinCos((float)(BodyRotX+TotalXBal));
SinG = SinA;
CosG = CosA;
GetSinCos((float)(BodyRotZ+TotalZBal));
SinB = SinA;
CosB = CosA;
GetSinCos((float)(BodyRotY+RotationY+TotalYBal));
//Calcualtion of rotation matrix:
BodyIKPosX = TotalX- (signed int)((float)(TotalX)*CosA*CosB - (float)(TotalZ)*CosB*SinA + (float)(PosY)*SinB);
BodyIKPosZ = TotalZ- (signed int)((float)(TotalX)*CosG*SinA + (float)(TotalX)*CosA*SinB*SinG +(float)(TotalZ)*CosA*CosG-(float)(TotalZ)*SinA*SinB*SinG-(float)(PosY)*CosB*SinG);
BodyIKPosY = PosY - (signed int)((float)(TotalX)*SinA*SinG - (float)(TotalX)*CosA*CosG*SinB + (float)(TotalZ)*CosA*SinG + (float)(TotalZ)*CosG*SinA*SinB + (float)(PosY)*CosB*CosG);
return;
}
void doBodyRot(void) {
// Degres : BodyRot
signed int PosZ1;
// Rotation suivant Y
GetSinCos( (float)BodyRotY );
// BodyPosZ = cos( BodyRotY ) * ( (RotPoint / cos( BodyRotY )) - RotPoint);
PosZ1 = (signed int)( ((float)RotPoint) * ( 1.0 - CosA ));
BodyPosX = BodyPosXint + (signed int)( ((float)RotPoint) * SinA);
// Rotation suivant X
GetSinCos( (float)BodyRotX );
BodyPosZ = BodyPosZint + PosZ1 + (signed int)( ((float)RotPoint) * ( 1.0 - CosA ));
BodyPosY = BodyPosYint - (signed int)( ((float)RotPoint) * SinA);
return;
}
// ----------------------------------------------------------------------------
//
void UTL_LocomotionController::ControlOjbect( object_control_s* pObjctl, Vector3* pDesiredVelocity, fixed_t _DesiredSpeed )
{
ASSERT( pObjctl != NULL );
ASSERT( pDesiredVelocity != NULL );
Vector3 xDir, yDir, zDir;
FetchXVector( pObjctl->pShip->ObjPosition, &xDir );
FetchYVector( pObjctl->pShip->ObjPosition, &yDir );
FetchZVector( pObjctl->pShip->ObjPosition, &zDir );
geomv_t len = VctLenX( pDesiredVelocity );
// stop control if desired velocity is zero
if ( len <= GEOMV_VANISHING ) {
pObjctl->rot_x = 0;
pObjctl->rot_y = 0;
pObjctl->accel = -0.82;
#ifdef BOT_LOGFILES
BOT_MsgOut( "ControlOjbect() got dimishing desired velocity" );
#endif // BOT_LOGFILES
return;
} else {
Vector3 DesVelNorm;
DesVelNorm.X = FLOAT_TO_GEOMV( pDesiredVelocity->X / len );
DesVelNorm.Y = FLOAT_TO_GEOMV( pDesiredVelocity->Y / len );
DesVelNorm.Z = FLOAT_TO_GEOMV( pDesiredVelocity->Z / len );
geomv_t yaw_dot = DOT_PRODUCT( &DesVelNorm, &xDir );
geomv_t pitch_dot = DOT_PRODUCT( &DesVelNorm, &yDir );
geomv_t heading_dot = DOT_PRODUCT( &DesVelNorm, &zDir );
float fDesiredSpeed = FIXED_TO_FLOAT( _DesiredSpeed );
float fCurSpeed = FIXED_TO_FLOAT( pObjctl->pShip->CurSpeed );
float pitch = 0;
float yaw = 0;
// target is behind us
sincosval_s fullturn;
GetSinCos( DEG_TO_BAMS( 2 * m_nRelaxedHeadingAngle ), &fullturn );
//if ( heading_dot < 0.0f ) {
if ( heading_dot < -fullturn.cosval ) {
// we must initiate a turn, if not already in a turn
if ( !pObjctl->IsYaw() || !pObjctl->IsPitch() ) {
// default to random
yaw = (float)( RAND() % 3 ) - 1;
pitch = (float)( RAND() % 3 ) - 1;
// steer towards goal
if ( yaw_dot < -GEOMV_VANISHING ) {
yaw = OCT_YAW_LEFT;
} else if ( yaw_dot > GEOMV_VANISHING ) {
yaw = OCT_YAW_RIGHT;
}
if ( pitch_dot < -GEOMV_VANISHING ) {
pitch = OCT_PITCH_UP;
} else if ( pitch_dot > GEOMV_VANISHING ) {
pitch = OCT_PITCH_DOWN;
}
} else {
// reuse prev. turn information
pitch = pObjctl->rot_x;
yaw = pObjctl->rot_y;
}
// slow down, until in direction of target
// pObjctl->accel = heading_dot;
//pObjctl->accel = OCT_DECELERATE;
// also maintain min. speed during turns
if ( fCurSpeed > m_fMinSpeedTurn ) {
pObjctl->accel = -0.42;
}
} else {
// determine accel
if ( fDesiredSpeed > fCurSpeed ) {
// accelerate towards target
pObjctl->accel = OCT_ACCELERATE;
} else if ( fDesiredSpeed < fCurSpeed ) {
// decelerate towards target
pObjctl->accel = OCT_DECELERATE;
} else {
// no accel
pObjctl->accel = 0;
}
// heading must be inside of 5 degrees cone angle
sincosval_s sincosv;
GetSinCos( DEG_TO_BAMS( m_nRelaxedHeadingAngle ), &sincosv );
if ( yaw_dot < -sincosv.sinval ) {
yaw = OCT_YAW_LEFT;
} else if ( yaw_dot > sincosv.sinval ) {
yaw = OCT_YAW_RIGHT;
}
if ( pitch_dot < -sincosv.sinval ) {
pitch = OCT_PITCH_UP;
} else if ( pitch_dot > sincosv.sinval ) {
pitch = OCT_PITCH_DOWN;
}
// if heading outside of 30 degrees cone angle, we decelerate
GetSinCos( DEG_TO_BAMS( m_nFullSpeedHeading ), &sincosv );
bool_t bYawOutsideHeading = ( ( yaw_dot < -sincosv.sinval ) || ( yaw_dot > sincosv.sinval ) );
bool_t bPitchOutsideHeading = ( ( pitch_dot < -sincosv.sinval ) || ( pitch_dot > sincosv.sinval ) );
if ( bYawOutsideHeading || bPitchOutsideHeading ) {
// also maintain min. speed during turns
if ( fCurSpeed > min( m_fMinSpeedTurn, fDesiredSpeed ) ) {
// decelerate towards target
pObjctl->accel = OCT_DECELERATE;
}
}
}
#ifdef BOT_LOGFILES
BOT_MsgOut( "heading_dot: %f, yaw_dot: %f, pitch_dot: %f", heading_dot, yaw_dot, pitch_dot );
#endif // BOT_LOGFILES
//FIXME: oct must also handle slide horiz./vert.
pObjctl->rot_x = pitch;
pObjctl->rot_y = yaw;
}
}
// draw currently selected ship in spacecraft viewer --------------------------
//
void DrawObjectsRing()
{
// spin object ring
if ( scv_ring_spinning != 0 ) {
MaintainObjectRingSpinning();
}
DoObjectsRingSliding();
// check whether actual drawing disabled
if ( AUX_DISABLE_FLOATING_MENU_DRAWING )
return;
// open direct object rendering
R_DirectObjectRendering( 0x01 );
for ( int oid = 0; oid < NUM_SCV_OBJECTS; oid++ ) {
// fetch current object class id in correct drawing order
int extraindex = scv_ring_objects[ scv_drawing_order[ ( oid ) % NUM_SCV_OBJECTS ] ];
dword classid = ExtraClasses[ extraindex ];
if ( classid == CLASS_ID_INVALID ) {
continue;
}
GenObject *objpo = ObjClasses[ classid ];
// translate object to its position on the ring
bams_t angle = ( RING_ARC * ( oid + scv_object_selindx ) ) +
scv_ring_spin_ofs + scv_ring_correction_ofs;
sincosval_s sincosv;
GetSinCos( angle, &sincosv );
float sinus = GEOMV_TO_FLOAT( sincosv.sinval );
float cosinus = GEOMV_TO_FLOAT( sincosv.cosval );
float objposx = SCV_SHIP_CENTER_X + ( sinus * RING_RADIUS ) - 5.0f;
if ( scv_ring_slidepos != -1 )
objposx += m_sintab[ scv_ring_slidepos ];
objpo->ObjPosition[ 0 ][ 3 ] = FLOAT_TO_GEOMV( objposx );
objpo->ObjPosition[ 1 ][ 3 ] = FLOAT_TO_GEOMV( SCV_SHIP_CENTER_Z - 27.0 );
objpo->ObjPosition[ 2 ][ 3 ] = FLOAT_TO_GEOMV( SCV_SHIP_CENTER_Y + ( cosinus * RING_RADIUS ) );
// rotate object
bams_t objview_rot = SCV_OBJROTSPEED * CurScreenRefFrames;
ObjRotX( objpo->ObjPosition, objview_rot );
ObjRotY( objpo->ObjPosition, objview_rot );
ObjRotZ( objpo->ObjPosition, objview_rot );
// reorthogonalize position matrix
if ( !AUX_DISABLE_REORTHO_IN_VIEWERS ) {
ReOrthoNormMtx( objpo->ObjPosition );
}
// render object
OBJ_AutoSelectObjectLod( objpo );
R_ReCalcAndRenderObject( objpo, SpacecraftCamera );
}
// close direct object rendering
R_DirectObjectRendering( 0x00 );
}