/*
=================
idAngles::ToRotation
=================
*/
idRotation idAngles::ToRotation() const
{
idVec3 vec;
float angle, w;
float sx, cx, sy, cy, sz, cz;
float sxcy, cxcy, sxsy, cxsy;
if( pitch == 0.0f )
{
if( yaw == 0.0f )
{
return idRotation( vec3_origin, idVec3( -1.0f, 0.0f, 0.0f ), roll );
}
if( roll == 0.0f )
{
return idRotation( vec3_origin, idVec3( 0.0f, 0.0f, -1.0f ), yaw );
}
}
else if( yaw == 0.0f && roll == 0.0f )
{
return idRotation( vec3_origin, idVec3( 0.0f, -1.0f, 0.0f ), pitch );
}
idMath::SinCos( DEG2RAD( yaw ) * 0.5f, sz, cz );
idMath::SinCos( DEG2RAD( pitch ) * 0.5f, sy, cy );
idMath::SinCos( DEG2RAD( roll ) * 0.5f, sx, cx );
sxcy = sx * cy;
cxcy = cx * cy;
sxsy = sx * sy;
cxsy = cx * sy;
vec.x = cxsy * sz - sxcy * cz;
vec.y = -cxsy * cz - sxcy * sz;
vec.z = sxsy * cz - cxcy * sz;
w = cxcy * cz + sxsy * sz;
angle = idMath::ACos( w );
if( angle == 0.0f )
{
vec.Set( 0.0f, 0.0f, 1.0f );
}
else
{
//vec *= (1.0f / sin( angle ));
vec.Normalize();
vec.FixDegenerateNormal();
angle *= 2.0f * idMath::M_RAD2DEG;
}
return idRotation( vec3_origin, vec, angle );
}
/*
================
idPhysics_Base::DrawVelocity
================
*/
void idPhysics_Base::DrawVelocity( int id, float linearScale, float angularScale ) const
{
idVec3 dir, org, vec, start, end;
idMat3 axis;
float length, a;
dir = GetLinearVelocity( id );
dir *= linearScale;
if( dir.LengthSqr() > Square( 0.1f ) )
{
dir = dir.Truncate( 10.0f );
org = GetOrigin( id );
gameRenderWorld->DebugArrow( colorRed, org, org + dir, 1 );
}
dir = GetAngularVelocity( id );
length = dir.Normalize();
length *= angularScale;
if( length > 0.1f )
{
if( length < 60.0f )
{
length = 60.0f;
}
else if( length > 360.0f )
{
length = 360.0f;
}
axis = GetAxis( id );
vec = axis[2];
if( idMath::Fabs( dir * vec ) > 0.99f )
{
vec = axis[0];
}
vec -= vec * dir * vec;
vec.Normalize();
vec *= 4.0f;
start = org + vec;
for( a = 20.0f; a < length; a += 20.0f )
{
end = org + idRotation( vec3_origin, dir, -a ).ToMat3() * vec;
gameRenderWorld->DebugLine( colorBlue, start, end, 1 );
start = end;
}
end = org + idRotation( vec3_origin, dir, -length ).ToMat3() * vec;
gameRenderWorld->DebugArrow( colorBlue, start, end, 1 );
}
}
/*
================
idBarrel::BarrelThink
================
*/
void idBarrel::BarrelThink(void) {
bool wasAtRest, onGround;
float movedDistance, rotatedDistance, angle;
idVec3 curOrigin, gravityNormal, dir;
idMat3 curAxis, axis;
wasAtRest = IsAtRest();
// run physics
RunPhysics();
// only need to give the visual model an additional rotation if the physics were run
if (!wasAtRest) {
// current physics state
onGround = GetPhysics()->HasGroundContacts();
curOrigin = GetPhysics()->GetOrigin();
curAxis = GetPhysics()->GetAxis();
// if the barrel is on the ground
if (onGround) {
gravityNormal = GetPhysics()->GetGravityNormal();
dir = curOrigin - lastOrigin;
dir -= gravityNormal * dir * gravityNormal;
movedDistance = dir.LengthSqr();
// if the barrel moved and the barrel is not aligned with the gravity direction
if (movedDistance > 0.0f && idMath::Fabs(gravityNormal * curAxis[barrelAxis]) < 0.7f) {
// barrel movement since last think frame orthogonal to the barrel axis
movedDistance = idMath::Sqrt(movedDistance);
dir *= 1.0f / movedDistance;
movedDistance = (1.0f - idMath::Fabs(dir * curAxis[barrelAxis])) * movedDistance;
// get rotation about barrel axis since last think frame
angle = lastAxis[(barrelAxis + 1) % 3] * curAxis[(barrelAxis + 1) % 3];
angle = idMath::ACos(angle);
// distance along cylinder hull
rotatedDistance = angle * radius;
// if the barrel moved further than it rotated about it's axis
if (movedDistance > rotatedDistance) {
// additional rotation of the visual model to make it look
// like the barrel rolls instead of slides
angle = 180.0f * (movedDistance - rotatedDistance) / (radius * idMath::PI);
if (gravityNormal.Cross(curAxis[barrelAxis]) * dir < 0.0f) {
additionalRotation += angle;
}
else {
additionalRotation -= angle;
}
dir = vec3_origin;
dir[barrelAxis] = 1.0f;
additionalAxis = idRotation(vec3_origin, dir, additionalRotation).ToMat3();
}
}
}
// save state for next think
lastOrigin = curOrigin;
lastAxis = curAxis;
}
Present();
}
/*
=====================
idQuat::ToRotation
=====================
*/
idRotation idQuat::ToRotation( void ) const {
idVec3 vec;
float angle;
vec.x = x;
vec.y = y;
vec.z = z;
angle = idMath::ACos( w );
if( angle == 0.0f ) {
vec.Set( 0.0f, 0.0f, 1.0f );
} else {
//vec *= (1.0f / sin( angle ));
vec.Normalize();
vec.FixDegenerateNormal();
angle *= 2.0f * idMath::M_RAD2DEG;
}
return idRotation( vec3_origin, vec, angle );
}
/*
================
idAF::LoadConstraint
================
*/
bool idAF::LoadConstraint( const idDeclAF_Constraint *fc )
{
idAFBody *body1, *body2;
idAngles angles;
idMat3 axis;
body1 = physicsObj.GetBody( fc->body1 );
body2 = physicsObj.GetBody( fc->body2 );
switch( fc->type )
{
case DECLAF_CONSTRAINT_FIXED:
{
idAFConstraint_Fixed *c;
c = static_cast<idAFConstraint_Fixed *>( physicsObj.GetConstraint( fc->name ) );
if( c )
{
c->SetBody1( body1 );
c->SetBody2( body2 );
}
else
{
c = new idAFConstraint_Fixed( fc->name, body1, body2 );
physicsObj.AddConstraint( c );
}
break;
}
case DECLAF_CONSTRAINT_BALLANDSOCKETJOINT:
{
idAFConstraint_BallAndSocketJoint *c;
c = static_cast<idAFConstraint_BallAndSocketJoint *>( physicsObj.GetConstraint( fc->name ) );
if( c )
{
c->SetBody1( body1 );
c->SetBody2( body2 );
}
else
{
c = new idAFConstraint_BallAndSocketJoint( fc->name, body1, body2 );
physicsObj.AddConstraint( c );
}
c->SetAnchor( fc->anchor.ToVec3() );
c->SetFriction( fc->friction );
switch( fc->limit )
{
case idDeclAF_Constraint::LIMIT_CONE:
{
c->SetConeLimit( fc->limitAxis.ToVec3(), fc->limitAngles[0], fc->shaft[0].ToVec3() );
break;
}
case idDeclAF_Constraint::LIMIT_PYRAMID:
{
angles = fc->limitAxis.ToVec3().ToAngles();
angles.roll = fc->limitAngles[2];
axis = angles.ToMat3();
c->SetPyramidLimit( axis[0], axis[1], fc->limitAngles[0], fc->limitAngles[1], fc->shaft[0].ToVec3() );
break;
}
default:
{
c->SetNoLimit();
break;
}
}
break;
}
case DECLAF_CONSTRAINT_UNIVERSALJOINT:
{
idAFConstraint_UniversalJoint *c;
c = static_cast<idAFConstraint_UniversalJoint *>( physicsObj.GetConstraint( fc->name ) );
if( c )
{
c->SetBody1( body1 );
c->SetBody2( body2 );
}
else
{
c = new idAFConstraint_UniversalJoint( fc->name, body1, body2 );
physicsObj.AddConstraint( c );
}
c->SetAnchor( fc->anchor.ToVec3() );
c->SetShafts( fc->shaft[0].ToVec3(), fc->shaft[1].ToVec3() );
c->SetFriction( fc->friction );
switch( fc->limit )
{
case idDeclAF_Constraint::LIMIT_CONE:
{
c->SetConeLimit( fc->limitAxis.ToVec3(), fc->limitAngles[0] );
break;
}
case idDeclAF_Constraint::LIMIT_PYRAMID:
{
angles = fc->limitAxis.ToVec3().ToAngles();
angles.roll = fc->limitAngles[2];
axis = angles.ToMat3();
c->SetPyramidLimit( axis[0], axis[1], fc->limitAngles[0], fc->limitAngles[1] );
break;
}
default:
{
c->SetNoLimit();
break;
}
}
break;
}
case DECLAF_CONSTRAINT_HINGE:
{
idAFConstraint_Hinge *c;
c = static_cast<idAFConstraint_Hinge *>( physicsObj.GetConstraint( fc->name ) );
if( c )
{
c->SetBody1( body1 );
c->SetBody2( body2 );
}
else
{
c = new idAFConstraint_Hinge( fc->name, body1, body2 );
physicsObj.AddConstraint( c );
}
c->SetAnchor( fc->anchor.ToVec3() );
c->SetAxis( fc->axis.ToVec3() );
c->SetFriction( fc->friction );
switch( fc->limit )
{
case idDeclAF_Constraint::LIMIT_CONE:
{
idVec3 left, up, newaxis, shaft;
fc->axis.ToVec3().OrthogonalBasis( left, up );
newaxis = left * idRotation( vec3_origin, fc->axis.ToVec3(), fc->limitAngles[0] );
shaft = left * idRotation( vec3_origin, fc->axis.ToVec3(), fc->limitAngles[2] );
c->SetLimit( newaxis, fc->limitAngles[1], shaft );
break;
}
default:
{
c->SetNoLimit();
break;
}
}
break;
}
case DECLAF_CONSTRAINT_SLIDER:
{
idAFConstraint_Slider *c;
c = static_cast<idAFConstraint_Slider *>( physicsObj.GetConstraint( fc->name ) );
if( c )
{
c->SetBody1( body1 );
c->SetBody2( body2 );
}
else
{
c = new idAFConstraint_Slider( fc->name, body1, body2 );
physicsObj.AddConstraint( c );
}
c->SetAxis( fc->axis.ToVec3() );
break;
}
case DECLAF_CONSTRAINT_SPRING:
{
idAFConstraint_Spring *c;
c = static_cast<idAFConstraint_Spring *>( physicsObj.GetConstraint( fc->name ) );
if( c )
{
c->SetBody1( body1 );
c->SetBody2( body2 );
}
else
{
c = new idAFConstraint_Spring( fc->name, body1, body2 );
physicsObj.AddConstraint( c );
}
c->SetAnchor( fc->anchor.ToVec3(), fc->anchor2.ToVec3() );
c->SetSpring( fc->stretch, fc->compress, fc->damping, fc->restLength );
c->SetLimit( fc->minLength, fc->maxLength );
break;
}
}
return true;
}
