void dgBody::ApplyImpulsesAtPoint (dgInt32 count, dgInt32 strideInBytes, const dgFloat32* const impulseArray, const dgFloat32* const pointArray)
{
dgInt32 stride = strideInBytes / sizeof (dgFloat32);
dgMatrix inertia (CalculateInertiaMatrix());
dgVector impulse (m_veloc.Scale3 (m_mass.m_w));
dgVector angularImpulse (inertia.RotateVector (m_omega));
dgVector com (m_globalCentreOfMass);
for (dgInt32 i = 0; i < count; i ++) {
dgInt32 index = i * stride;
dgVector r (pointArray[index], pointArray[index + 1], pointArray[index + 2], dgFloat32 (0.0f));
dgVector L (impulseArray[index], impulseArray[index + 1], impulseArray[index + 2], dgFloat32 (0.0f));
dgVector Q ((r - com) * L);
impulse += L;
angularImpulse += Q;
}
dgMatrix invInertia (CalculateInvInertiaMatrix());
m_veloc = impulse.Scale3(m_invMass.m_w);
m_omega = invInertia.RotateVector(angularImpulse);
m_sleeping = false;
m_equilibrium = false;
Unfreeze ();
}
//-------------------------------------------------------------------------
static MH_STATUS EnableAllHooksLL(BOOL enable)
{
UINT i;
for (i = 0; i < g_hooks.size; ++i)
{
if (g_hooks.pItems[i].isEnabled != enable)
{
FROZEN_THREADS threads;
Freeze(&threads, ALL_HOOKS_POS, enable ? ACTION_ENABLE : ACTION_DISABLE);
__try
{
for (; i < g_hooks.size; ++i)
{
if (g_hooks.pItems[i].isEnabled != enable)
{
MH_STATUS status = EnableHookLL(i, enable);
if (status != MH_OK)
return status;
}
}
}
__finally
{
Unfreeze(&threads);
}
}
}
//-------------------------------------------------------------------------
static MH_STATUS EnableAllHooksLL(BOOL enable)
{
MH_STATUS status = MH_OK;
UINT i, first = INVALID_HOOK_POS;
for (i = 0; i < g_hooks.size; ++i)
{
if (g_hooks.pItems[i].isEnabled != enable)
{
first = i;
break;
}
}
if (first != INVALID_HOOK_POS)
{
FROZEN_THREADS threads;
Freeze(&threads, ALL_HOOKS_POS, enable ? ACTION_ENABLE : ACTION_DISABLE);
for (i = first; i < g_hooks.size; ++i)
{
if (g_hooks.pItems[i].isEnabled != enable)
{
status = EnableHookLL(i, enable);
if (status != MH_OK)
break;
}
}
Unfreeze(&threads);
}
return status;
}
bool8 S9xUnfreezeGame(const char* filename)
{
if (statef_open(filename, "rb"))
{
int result;
if ((result = Unfreeze()) != SUCCESS)
{
switch (result)
{
case WRONG_FORMAT:
S9xMessage(S9X_ERROR, S9X_WRONG_FORMAT,
"File not in Snes9x freeze format");
S9xReset();
break;
case WRONG_VERSION:
S9xMessage(S9X_ERROR, S9X_WRONG_VERSION,
"Incompatable Snes9x freeze file format version");
S9xReset();
break;
default:
// should never happen
break;
}
statef_close();
return (FALSE);
}
statef_close();
return (TRUE);
}
return (FALSE);
}
//-------------------------------------------------------------------------
static MH_STATUS EnableAllHooksLL(BOOL enable)
{
int i;
for (i = 0; i < g_hooks.size; ++i)
{
if (g_hooks.items[i].isEnabled != enable)
{
Freeze(-1, 1);
__try
{
for (; i < g_hooks.size; ++i)
{
if (g_hooks.items[i].isEnabled != enable)
{
MH_STATUS status = EnableHookLL(i, enable);
if (status != MH_OK)
return status;
}
}
}
__finally
{
Unfreeze();
}
}
}
void dgBody::SetFreeze (bool state)
{
if (state){
Freeze();
} else {
Unfreeze();
}
}
void dgBody::AddImpulse (const dgVector& pointDeltaVeloc, const dgVector& pointPosit)
{
dgMatrix invInertia (CalculateInvInertiaMatrix());
// get contact matrix
dgMatrix tmp;
dgVector globalContact (pointPosit - m_globalCentreOfMass);
tmp[0][0] = dgFloat32 (0.0f);
tmp[0][1] = + globalContact[2];
tmp[0][2] = - globalContact[1];
tmp[0][3] = dgFloat32 (0.0f);
tmp[1][0] = -globalContact[2];
tmp[1][1] = dgFloat32 (0.0f);
tmp[1][2] = +globalContact[0];
tmp[1][3] = dgFloat32 (0.0f);
tmp[2][0] = +globalContact[1];
tmp[2][1] = -globalContact[0];
tmp[2][2] = dgFloat32 (0.0f);
tmp[2][3] = dgFloat32 (0.0f);
tmp[3][0] = dgFloat32 (0.0f);
tmp[3][1] = dgFloat32 (0.0f);
tmp[3][2] = dgFloat32 (0.0f);
tmp[3][3] = dgFloat32 (1.0f);
dgMatrix contactMatrix (tmp * invInertia * tmp);
for (dgInt32 i = 0; i < 3; i ++) {
for (dgInt32 j = 0; j < 3; j ++) {
contactMatrix[i][j] *= -dgFloat32 (1.0f);
}
}
contactMatrix[0][0] += m_invMass.m_w;
contactMatrix[1][1] += m_invMass.m_w;
contactMatrix[2][2] += m_invMass.m_w;
contactMatrix = contactMatrix.Symetric3by3Inverse ();
// change of momentum
dgVector changeOfMomentum (contactMatrix.RotateVector (pointDeltaVeloc));
dgVector dv (changeOfMomentum.Scale3 (m_invMass.m_w));
dgVector dw (invInertia.RotateVector (globalContact * changeOfMomentum));
m_veloc += dv;
m_omega += dw;
m_sleeping = false;
m_equilibrium = false;
Unfreeze ();
}
void dgBody::ApplyImpulsePair (const dgVector& linearImpulseIn, const dgVector& angularImpulseIn)
{
dgMatrix inertia (CalculateInertiaMatrix());
dgMatrix invInertia (CalculateInvInertiaMatrix());
dgVector linearImpulse (m_veloc.Scale3 (m_mass.m_w) + linearImpulseIn);
dgVector angularImpulse (inertia.RotateVector (m_omega) + angularImpulseIn);
m_veloc = linearImpulse.Scale3(m_invMass.m_w);
m_omega = invInertia.RotateVector(angularImpulse);
m_sleeping = false;
m_equilibrium = false;
Unfreeze ();
}
//-------------------------------------------------------------------------
MH_STATUS WINAPI MH_ApplyQueued(VOID)
{
MH_STATUS status = MH_OK;
UINT i, first = INVALID_HOOK_POS;
EnterSpinLock();
if (g_hHeap != NULL)
{
for (i = 0; i < g_hooks.size; ++i)
{
if (g_hooks.pItems[i].isEnabled != g_hooks.pItems[i].queueEnable)
{
first = i;
break;
}
}
if (first != INVALID_HOOK_POS)
{
FROZEN_THREADS threads;
Freeze(&threads, ALL_HOOKS_POS, ACTION_APPLY_QUEUED);
for (i = first; i < g_hooks.size; ++i)
{
PHOOK_ENTRY pHook = &g_hooks.pItems[i];
if (pHook->isEnabled != pHook->queueEnable)
{
status = EnableHookLL(i, pHook->queueEnable);
if (status != MH_OK)
break;
}
}
Unfreeze(&threads);
}
}
else
{
status = MH_ERROR_NOT_INITIALIZED;
}
LeaveSpinLock();
return status;
}
//-------------------------------------------------------------------------
static MH_STATUS EnableHook(LPVOID pTarget, BOOL enable)
{
MH_STATUS status = MH_OK;
EnterSpinLock();
if (g_hHeap != NULL)
{
if (pTarget == MH_ALL_HOOKS)
{
status = EnableAllHooksLL(enable);
}
else
{
FROZEN_THREADS threads;
UINT pos = FindHookEntry(pTarget);
if (pos != INVALID_HOOK_POS)
{
if (g_hooks.pItems[pos].isEnabled != enable)
{
Freeze(&threads, pos, ACTION_ENABLE);
status = EnableHookLL(pos, enable);
Unfreeze(&threads);
}
else
{
status = enable ? MH_ERROR_ENABLED : MH_ERROR_DISABLED;
}
}
else
{
status = MH_ERROR_NOT_CREATED;
}
}
}
else
{
status = MH_ERROR_NOT_INITIALIZED;
}
LeaveSpinLock();
return status;
}
//-------------------------------------------------------------------------
MH_STATUS WINAPI MH_RemoveHook(LPVOID pTarget)
{
MH_STATUS status = MH_OK;
EnterSpinLock();
if (g_hHeap != NULL)
{
UINT pos = FindHookEntry(pTarget);
if (pos != INVALID_HOOK_POS)
{
if (g_hooks.pItems[pos].isEnabled)
{
FROZEN_THREADS threads;
Freeze(&threads, pos, ACTION_DISABLE);
status = EnableHookLL(pos, FALSE);
Unfreeze(&threads);
}
if (status == MH_OK)
{
FreeBuffer(g_hooks.pItems[pos].pTrampoline);
DeleteHookEntry(pos);
}
}
else
{
status = MH_ERROR_NOT_CREATED;
}
}
else
{
status = MH_ERROR_NOT_INITIALIZED;
}
LeaveSpinLock();
return status;
}
ScopedThreadExclusive::~ScopedThreadExclusive()
{
Unfreeze(threads_);
}
