void CStatueProp::VPhysicsUpdate( IPhysicsObject *pPhysics )
{
BaseClass::VPhysicsUpdate( pPhysics );
if ( s_vcollide_wireframe->GetBool() )
{
const CPhysCollide *pCollide = pPhysics->GetCollide();
Vector vecOrigin;
QAngle angAngles;
pPhysics->GetPosition( &vecOrigin, &angAngles );
if ( pCollide )
{
Vector *outVerts;
int vertCount = physcollision->CreateDebugMesh( pCollide, &outVerts );
int triCount = vertCount / 3;
int vert = 0;
VMatrix tmp = SetupMatrixOrgAngles( vecOrigin, angAngles );
int i;
for ( i = 0; i < vertCount; i++ )
{
outVerts[i] = tmp.VMul4x3( outVerts[i] );
}
for ( i = 0; i < triCount; i++ )
{
NDebugOverlay::Line( outVerts[ vert + 0 ], outVerts[ vert + 1 ], 0, 255, 255, false, 0.0f );
NDebugOverlay::Line( outVerts[ vert + 1 ], outVerts[ vert + 2 ], 0, 255, 255, false, 0.0f );
NDebugOverlay::Line( outVerts[ vert + 2 ], outVerts[ vert + 0 ], 0, 255, 255, false, 0.0f );
vert += 3;
}
physcollision->DestroyDebugMesh( vertCount, outVerts );
}
}
}
IMotionEvent::simresult_e CGravControllerPoint::Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular )
{
Vector vel;
AngularImpulse angVel;
float fracRemainingSimTime = 1.0;
if ( m_timeToArrive > 0 )
{
fracRemainingSimTime *= deltaTime / m_timeToArrive;
if ( fracRemainingSimTime > 1 )
{
fracRemainingSimTime = 1;
}
}
m_timeToArrive -= deltaTime;
if ( m_timeToArrive < 0 )
{
m_timeToArrive = 0;
}
float invDeltaTime = (1.0f / deltaTime);
Vector world;
pObject->LocalToWorld( &world, m_localPosition );
m_worldPosition = world;
pObject->GetVelocity( &vel, &angVel );
//pObject->GetVelocityAtPoint( world, &vel );
float damping = 1.0;
world += vel * deltaTime * damping;
Vector delta = (m_targetPosition - world) * fracRemainingSimTime * invDeltaTime;
Vector alignDir;
linear = vec3_origin;
angular = vec3_origin;
if ( m_align )
{
QAngle angles;
Vector origin;
Vector axis;
AngularImpulse torque;
pObject->GetShadowPosition( &origin, &angles );
// align local normal to target normal
VMatrix tmp = SetupMatrixOrgAngles( origin, angles );
Vector worldNormal = tmp.VMul3x3( m_localAlignNormal );
axis = CrossProduct( worldNormal, m_targetAlignNormal );
float trig = VectorNormalize(axis);
float alignRotation = RAD2DEG(asin(trig));
axis *= alignRotation;
if ( alignRotation < 10 )
{
float dot = DotProduct( worldNormal, m_targetAlignNormal );
// probably 180 degrees off
if ( dot < 0 )
{
if ( worldNormal.x < 0.5 )
{
axis.Init(10,0,0);
}
else
{
axis.Init(0,0,10);
}
alignRotation = 10;
}
}
// Solve for the rotation around the target normal (at the local align pos) that will
// move the grabbed spot to the destination.
Vector worldRotCenter = tmp.VMul4x3( m_localAlignPosition );
Vector rotSrc = world - worldRotCenter;
Vector rotDest = m_targetPosition - worldRotCenter;
// Get a basis in the plane perpendicular to m_targetAlignNormal
Vector srcN = rotSrc;
VectorNormalize( srcN );
Vector tangent = CrossProduct( srcN, m_targetAlignNormal );
float len = VectorNormalize( tangent );
// needs at least ~5 degrees, or forget rotation (0.08 ~= sin(5))
if ( len > 0.08 )
{
Vector binormal = CrossProduct( m_targetAlignNormal, tangent );
// Now project the src & dest positions into that plane
Vector planeSrc( DotProduct( rotSrc, tangent ), DotProduct( rotSrc, binormal ), 0 );
Vector planeDest( DotProduct( rotDest, tangent ), DotProduct( rotDest, binormal ), 0 );
float rotRadius = VectorNormalize( planeSrc );
float destRadius = VectorNormalize( planeDest );
if ( rotRadius > 0.1 )
{
if ( destRadius < rotRadius )
{
destRadius = rotRadius;
}
//float ratio = rotRadius / destRadius;
float angleSrc = atan2( planeSrc.y, planeSrc.x );
float angleDest = atan2( planeDest.y, planeDest.x );
float angleDiff = angleDest - angleSrc;
angleDiff = RAD2DEG(angleDiff);
axis += m_targetAlignNormal * angleDiff;
//world = m_targetPosition;// + rotDest * (1-ratio);
// NDebugOverlay::Line( worldRotCenter, worldRotCenter-m_targetAlignNormal*50, 255, 0, 0, false, 0.1 );
// NDebugOverlay::Line( worldRotCenter, worldRotCenter+tangent*50, 0, 255, 0, false, 0.1 );
// NDebugOverlay::Line( worldRotCenter, worldRotCenter+binormal*50, 0, 0, 255, false, 0.1 );
}
}
torque = WorldToLocalRotation( tmp, axis, 1 );
torque *= fracRemainingSimTime * invDeltaTime;
torque -= angVel * 1.0; // damping
for ( int i = 0; i < 3; i++ )
{
if ( torque[i] > 0 )
{
if ( torque[i] > m_maxAngularAcceleration[i] )
torque[i] = m_maxAngularAcceleration[i];
}
else
{
if ( torque[i] < -m_maxAngularAcceleration[i] )
torque[i] = -m_maxAngularAcceleration[i];
}
}
torque *= invDeltaTime;
angular += torque;
// Calculate an acceleration that pulls the object toward the constraint
// When you're out of alignment, don't pull very hard
float factor = fabsf(alignRotation);
if ( factor < 5 )
{
factor = clamp( factor, 0, 5 ) * (1/5);
alignDir = m_targetAlignPosition - worldRotCenter;
// Limit movement to the part along m_targetAlignNormal if worldRotCenter is on the backside of
// of the target plane (one inch epsilon)!
float planeForward = DotProduct( alignDir, m_targetAlignNormal );
if ( planeForward > 1 )
{
alignDir = m_targetAlignNormal * planeForward;
}
Vector accel = alignDir * invDeltaTime * fracRemainingSimTime * (1-factor) * 0.20 * invDeltaTime;
float mag = accel.Length();
if ( mag > m_maxAcceleration )
{
accel *= (m_maxAcceleration/mag);
}
linear += accel;
}
linear -= vel*damping*invDeltaTime;
// UNDONE: Factor in the change in worldRotCenter due to applied torque!
}
else
{
// clamp future velocity to max speed
Vector nextVel = delta + vel;
float nextSpeed = nextVel.Length();
if ( nextSpeed > m_maxVel )
{
nextVel *= (m_maxVel / nextSpeed);
delta = nextVel - vel;
}
delta *= invDeltaTime;
float linearAccel = delta.Length();
if ( linearAccel > m_maxAcceleration )
{
delta *= m_maxAcceleration / linearAccel;
}
Vector accel;
AngularImpulse angAccel;
pObject->CalculateForceOffset( delta, world, &accel, &angAccel );
linear += accel;
angular += angAccel;
}
return SIM_GLOBAL_ACCELERATION;
}