DG_INLINE void InitKinematicMassMatrix (const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgFloat32 correctionFactor)
{
dgAssert (0);
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
m_bodyForce.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
m_jointForce[m_dof] = -dgClamp(row->m_penetration * correctionFactor, dgFloat32(-0.25f), dgFloat32(0.25f));
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof++;
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
DG_INLINE void Factorize(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32 (0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert (m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
if (((row->m_lowerBoundFrictionCoefficent < dgFloat32 (-1.0e9f)) && row->m_upperBoundFrictionCoefficent > dgFloat32 (1.0e9f))) {
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof ++;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
void CPhysicsObject::OutputDebugInfo() const {
Msg("-----------------\n");
if (m_pName)
Msg("Object: %s\n", m_pName);
Msg("Mass: %f (inv %f)\n", GetMass(), GetInvMass());
Vector pos;
QAngle ang;
GetPosition(&pos, &ang);
Msg("Position: %f %f %f\nAngle: %f %f %f\n", pos.x, pos.y, pos.z, ang.x, ang.y, ang.z);
Vector inertia = GetInertia();
Vector invinertia = GetInvInertia();
Msg("Inertia: %f %f %f (inv %f %f %f)\n", inertia.x, inertia.y, inertia.z, invinertia.x, invinertia.y, invinertia.z);
Vector vel;
AngularImpulse angvel;
GetVelocity(&vel, &angvel);
Msg("Velocity: %f, %f, %f\nAng Velocity: %f, %f, %f\n", vel.x, vel.y, vel.z, angvel.x, angvel.y, angvel.z);
float dampspeed, damprot;
GetDamping(&dampspeed, &damprot);
Msg("Damping %f linear, %f angular\n", dampspeed, damprot);
Vector dragBasis;
Vector angDragBasis;
ConvertPosToHL(m_dragBasis, dragBasis);
ConvertDirectionToHL(m_angDragBasis, angDragBasis);
Msg("Linear Drag: %f, %f, %f (factor %f)\n", dragBasis.x, dragBasis.y, dragBasis.z, m_dragCoefficient);
Msg("Angular Drag: %f, %f, %f (factor %f)\n", angDragBasis.x, angDragBasis.y, angDragBasis.z, m_angDragCoefficient);
// TODO: Attached to x controllers
Msg("State: %s, Collision %s, Motion %s, Drag %s, Flags %04X (game %04x, index %d)\n",
IsAsleep() ? "Asleep" : "Awake",
IsCollisionEnabled() ? "Enabled" : "Disabled",
IsStatic() ? "Static" : IsMotionEnabled() ? "Enabled" : "Disabled",
IsDragEnabled() ? "Enabled" : "Disabled",
m_pObject->getFlags(),
GetGameFlags(),
GetGameIndex()
);
const char *pMaterialStr = g_SurfaceDatabase.GetPropName(m_materialIndex);
surfacedata_t *surfaceData = g_SurfaceDatabase.GetSurfaceData(m_materialIndex);
if (surfaceData) {
Msg("Material: %s : density(%f), thickness(%f), friction(%f), elasticity(%f)\n",
pMaterialStr, surfaceData->physics.density, surfaceData->physics.thickness, surfaceData->physics.friction, surfaceData->physics.elasticity);
}
Msg("-- COLLISION SHAPE INFO --\n");
g_PhysicsCollision.OutputDebugInfo((CPhysCollide *)m_pObject->getCollisionShape()->getUserPointer());
}
DG_INLINE void FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow, dgForcePair& accel)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
const dgInt32 first = jointInfo->m_pairStart;
dgInt32 clampedValue = m_dof - 1;
//dgSpatialVector& accel = force.m_joint;
for (dgInt32 i = 0; i < m_dof; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
//dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar();
dgFloat32 force = row->m_force;
if ((force >= row->m_lowerBoundFrictionCoefficent) && (force <= row->m_upperBoundFrictionCoefficent)) {
dgVector residual(row->m_JMinv.m_jacobianM0.m_linear.CompProduct4(y0.m_linear) + row->m_JMinv.m_jacobianM0.m_angular.CompProduct4(y0.m_angular) +
row->m_JMinv.m_jacobianM1.m_linear.CompProduct4(y1.m_linear) + row->m_JMinv.m_jacobianM1.m_angular.CompProduct4(y1.m_angular));
residual = dgVector(row->m_coordenateAccel) - residual.AddHorizontal();
accel.m_joint[i] = -residual.GetScalar();
} else {
dgAssert (0);
dgSwap (m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i --;
m_dof --;
clampedValue --;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
/*********************
* Calc accelerations *
*********************/
void pEngine::CalcAccelerations()
{
#ifdef LTRACE
qdbg("pEngine:CalcAcc()\n");
#endif
if(!driveLine->IsPrePostLocked())
{
// Engine moves separately from the rest of the driveLine
rotA=(tEngine-driveLine->GetClutchTorque())/GetInertia();
#ifdef LTRACE
qdbg(" tEngine=%.2f, tClutch=%.2f, rotA=%.2f\n",tEngine,driveLine->GetClutchTorque(),rotA);
#endif
} // else rotA is calculated in the driveline (passed up
// from the wheels on to the root - the engine)
}
DG_INLINE void UpdateFactorizeLCP(const dgJointInfo* const jointInfoArray, dgJacobianMatrixElement* const matrixRow, dgForcePair& force, dgForcePair& accel)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 first = jointInfo->m_pairStart;
dgInt32 clampedValue = m_dof - 1;
for (dgInt32 i = 0; i < m_dof; i++) {
dgInt32 k = m_sourceJacobianIndex[i];
const dgJacobianMatrixElement* const row = &matrixRow[first + k];
dgFloat32 f = force.m_joint[i] + row->m_force;
if (f < row->m_lowerBoundFrictionCoefficent) {
force.m_joint[i] = dgFloat32 (0.0f);
dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]);
dgSwap(force.m_joint[i], force.m_joint[clampedValue]);
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i--;
m_dof--;
clampedValue--;
} else if (f > row->m_upperBoundFrictionCoefficent) {
force.m_joint[i] = dgFloat32 (0.0f);
dgSwap(accel.m_joint[i], accel.m_joint[clampedValue]);
dgSwap(force.m_joint[i], force.m_joint[clampedValue]);
dgSwap(m_sourceJacobianIndex[i], m_sourceJacobianIndex[clampedValue]);
i--;
m_dof--;
clampedValue--;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChChassis::Initialize(ChSystem* system,
const ChCoordsys<>& chassisPos,
double chassisFwdVel,
int collision_family) {
m_body = std::shared_ptr<ChBodyAuxRef>(system->NewBodyAuxRef());
m_body->SetIdentifier(0);
m_body->SetNameString(m_name + "_body");
m_body->SetMass(GetMass());
m_body->SetFrame_COG_to_REF(ChFrame<>(GetLocalPosCOM(), ChQuaternion<>(1, 0, 0, 0)));
m_body->SetInertia(GetInertia());
m_body->SetBodyFixed(m_fixed);
m_body->SetFrame_REF_to_abs(ChFrame<>(chassisPos));
m_body->SetPos_dt(chassisFwdVel * chassisPos.TransformDirectionLocalToParent(ChVector<>(1, 0, 0)));
system->Add(m_body);
}
DG_INLINE void FactorizeLCP(const dgJointInfo* const jointInfoArray, const dgJacobian* const internalForces, dgJacobianMatrixElement* const matrixRow)
{
dgAssert((dgUnsigned64(&m_bodyMass) & 0x0f) == 0);
m_bodyMass.SetZero();
if (m_body->GetInvMass().m_w != dgFloat32(0.0f)) {
GetInertia();
}
if (m_joint) {
dgAssert(m_parent);
const dgJointInfo* const jointInfo = &jointInfoArray[m_joint->m_index];
dgAssert(jointInfo->m_joint == m_joint);
dgAssert(jointInfo->m_joint->GetBody0() == m_body);
dgAssert(jointInfo->m_joint->GetBody1() == m_parent->m_body);
const dgInt32 m0 = jointInfo->m_m0;
const dgInt32 m1 = jointInfo->m_m1;
const dgJacobian& y0 = internalForces[m0];
const dgJacobian& y1 = internalForces[m1];
m_dof = 0;
const dgInt32 count = jointInfo->m_pairCount;
const dgInt32 first = jointInfo->m_pairStart;
for (dgInt32 i = 0; i < count; i++) {
const dgJacobianMatrixElement* const row = &matrixRow[i + first];
dgVector residual(row->m_JMinv.m_jacobianM0.m_linear.CompProduct4(y0.m_linear) + row->m_JMinv.m_jacobianM0.m_angular.CompProduct4(y0.m_angular) +
row->m_JMinv.m_jacobianM1.m_linear.CompProduct4(y1.m_linear) + row->m_JMinv.m_jacobianM1.m_angular.CompProduct4(y1.m_angular));
residual = dgVector(row->m_coordenateAccel) - residual.AddHorizontal();
dgFloat32 force = row->m_force + row->m_invJMinvJt * residual.GetScalar();
if ((force > row->m_lowerBoundFrictionCoefficent) && (force < row->m_upperBoundFrictionCoefficent)) {
m_sourceJacobianIndex[m_dof] = dgInt8(i);
m_dof++;
}
}
GetJacobians(jointInfo, matrixRow);
}
Factorize();
}
Grenade::Grenade(const TVector2D& src, const TVector2D& velocity, unsigned int _owner) :
m_iOwner(_owner),
m_iCurrentFrame(0),
m_fTimerChangeFrame(0.0f),
m_bPlayOnlyOnce(true),
m_fTimerThrow(world.getCurrentTime),
killMyself(false)
{
m_xTexture = &WeaponManager::GetSingleton().text_grenade[0];
m_fHalfHeight = m_xTexture->h/2.0f;
m_fHalfWidth = m_xTexture->w/2.0f;
Set(src, velocity, 8, m_fHalfHeight, m_fHalfWidth, 0.01f);
SetCollisionCallback(HandleContact);
GetInvInertia() = 0.0f;
GetInertia() = 0.0f;
GetAngVelocity() = 0.0f;
SetOrientation(0.0f);
type = TYPE_GRENADE;
}
void CPhysicsObject::WriteToTemplate( vphysics_save_cphysicsobject_t &objectTemplate )
{
if ( m_collideType == COLLIDE_BALL )
{
objectTemplate.pCollide = NULL;
objectTemplate.sphereRadius = GetSphereRadius();
}
else
{
objectTemplate.pCollide = GetCollide();
objectTemplate.sphereRadius = 0;
}
objectTemplate.isStatic = IsStatic();
objectTemplate.collisionEnabled = IsCollisionEnabled();
objectTemplate.gravityEnabled = IsGravityEnabled();
objectTemplate.dragEnabled = IsDragEnabled();
objectTemplate.motionEnabled = IsMotionEnabled();
objectTemplate.isAsleep = IsAsleep();
objectTemplate.isTrigger = IsTrigger();
objectTemplate.materialIndex = m_materialIndex;
objectTemplate.mass = GetMass();
objectTemplate.rotInertia = GetInertia();
GetDamping( &objectTemplate.speedDamping, &objectTemplate.rotSpeedDamping );
objectTemplate.massCenterOverride = m_massCenterOverride;
objectTemplate.callbacks = m_callbacks;
objectTemplate.gameFlags = m_gameFlags;
objectTemplate.volume = GetVolume();
objectTemplate.dragCoefficient = m_dragCoefficient;
objectTemplate.angDragCoefficient = m_angDragCoefficient;
objectTemplate.pShadow = m_pShadow;
objectTemplate.hasShadowController = (m_pShadow != NULL) ? true : false;
//bool m_shadowTempGravityDisable;
objectTemplate.collideType = m_collideType;
objectTemplate.contentsMask = m_contentsMask;
GetPosition( &objectTemplate.origin, &objectTemplate.angles );
GetVelocity( &objectTemplate.velocity, &objectTemplate.angVelocity );
}
