//
// XIDamageEffect::evolve
//
// Damage utilizes both motors to do effects that decay at different rates.
//
void XIDamageEffect::evolve(XINPUT_VIBRATION &xvib, uint32_t curTime)
{
if(checkDone(curTime))
{
delete this;
return;
}
// Do left motor processing
DWORD endStrength = strength * 40 / 64;
DWORD deltaStrength = (strength - endStrength);
DWORD curStrength = strength - deltaStrength * (curTime - startTime) / duration;
AddClamped(xvib, MOTOR_LEFT, static_cast<WORD>(curStrength));
// Do right motor processing
if(curTime - startTime < duration / 2)
{
// Right motor is constant during first half of effect
AddClamped(xvib, MOTOR_RIGHT, strength / 2);
}
else
{
// Linear descent to zero
curStrength = strength / 2;
curStrength -= curStrength * (curTime - startTime) / duration;
AddClamped(xvib, MOTOR_RIGHT, static_cast<WORD>(curStrength));
}
}
//
// XILinearEffect::evolve
//
// A linear effect ramps up or down from the initial value to the end value
// over a set course of time.
//
void XILinearEffect::evolve(XINPUT_VIBRATION &xvib, uint32_t curTime)
{
if(checkDone(curTime))
{
delete this;
return;
}
WORD curStrength;
if(direction < 0)
{
// slope down
WORD deltaStrength = (initStrength - endStrength);
curStrength = initStrength - deltaStrength * (curTime - startTime) / duration;
}
else
{
// slope up
WORD deltaStrength = (endStrength - initStrength);
curStrength = initStrength + deltaStrength * (curTime - startTime) / duration;
}
AddClamped(xvib, which, curStrength);
}
//
// XIConstantEffect::evolve
//
// Pulse the motor at a constant strength. Due to the behavior of the XBox360
// controller's non-solenoid motors however, there's a bit of "catch" which
// adds some unavoidable pseudo-random variance into the mix. Depending on
// where it stopped last time, you get no response at all until it seems to
// "roll over" the catch, at which point you get more than you asked for.
//
void XIConstantEffect::evolve(XINPUT_VIBRATION &xvib, uint32_t curTime)
{
if(checkDone(curTime))
{
delete this;
return;
}
AddClamped(xvib, which, strength);
}
//
// XIRumbleEffect::evolve
//
// The rumble effect is entirely chaotic within a given min to max range.
//
void XIRumbleEffect::evolve(XINPUT_VIBRATION &xvib, uint32_t curTime)
{
if(checkDone(curTime))
{
delete this;
return;
}
int minStr = minStrength;
int rndStr = abs(rand()) % (maxStrength - minStrength);
WORD totStr = static_cast<WORD>(minStr + rndStr);
AddClamped(xvib, which, totStr);
}
/*
============
idLCP_Square::Solve
============
*/
bool idLCP_Square::Solve(const idMatX &o_m, idVecX &o_x, const idVecX &o_b, const idVecX &o_lo, const idVecX &o_hi, const int *o_boxIndex)
{
int i, j, n, limit, limitSide, boxStartIndex;
float dir, maxStep, dot, s;
char *failed;
// true when the matrix rows are 16 byte padded
padded = ((o_m.GetNumRows()+3)&~3) == o_m.GetNumColumns();
assert(padded || o_m.GetNumRows() == o_m.GetNumColumns());
assert(o_x.GetSize() == o_m.GetNumRows());
assert(o_b.GetSize() == o_m.GetNumRows());
assert(o_lo.GetSize() == o_m.GetNumRows());
assert(o_hi.GetSize() == o_m.GetNumRows());
// allocate memory for permuted input
f.SetData(o_m.GetNumRows(), VECX_ALLOCA(o_m.GetNumRows()));
a.SetData(o_b.GetSize(), VECX_ALLOCA(o_b.GetSize()));
b.SetData(o_b.GetSize(), VECX_ALLOCA(o_b.GetSize()));
lo.SetData(o_lo.GetSize(), VECX_ALLOCA(o_lo.GetSize()));
hi.SetData(o_hi.GetSize(), VECX_ALLOCA(o_hi.GetSize()));
if (o_boxIndex) {
boxIndex = (int *)_alloca16(o_x.GetSize() * sizeof(int));
memcpy(boxIndex, o_boxIndex, o_x.GetSize() * sizeof(int));
} else {
boxIndex = NULL;
}
// we override the const on o_m here but on exit the matrix is unchanged
m.SetData(o_m.GetNumRows(), o_m.GetNumColumns(), const_cast<float *>(o_m[0]));
f.Zero();
a.Zero();
b = o_b;
lo = o_lo;
hi = o_hi;
// pointers to the rows of m
rowPtrs = (float **) _alloca16(m.GetNumRows() * sizeof(float *));
for (i = 0; i < m.GetNumRows(); i++) {
rowPtrs[i] = m[i];
}
// tells if a variable is at the low boundary, high boundary or inbetween
side = (int *) _alloca16(m.GetNumRows() * sizeof(int));
// index to keep track of the permutation
permuted = (int *) _alloca16(m.GetNumRows() * sizeof(int));
for (i = 0; i < m.GetNumRows(); i++) {
permuted[i] = i;
}
// permute input so all unbounded variables come first
numUnbounded = 0;
for (i = 0; i < m.GetNumRows(); i++) {
if (lo[i] == -idMath::INFINITY && hi[i] == idMath::INFINITY) {
if (numUnbounded != i) {
Swap(numUnbounded, i);
}
numUnbounded++;
}
}
// permute input so all variables using the boxIndex come last
boxStartIndex = m.GetNumRows();
if (boxIndex) {
for (i = m.GetNumRows() - 1; i >= numUnbounded; i--) {
if (boxIndex[i] >= 0 && (lo[i] != -idMath::INFINITY || hi[i] != idMath::INFINITY)) {
boxStartIndex--;
if (boxStartIndex != i) {
Swap(boxStartIndex, i);
}
}
}
}
// sub matrix for factorization
clamped.SetData(m.GetNumRows(), m.GetNumColumns(), MATX_ALLOCA(m.GetNumRows() * m.GetNumColumns()));
diagonal.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
// all unbounded variables are clamped
numClamped = numUnbounded;
// if there are unbounded variables
if (numUnbounded) {
// factor and solve for unbounded variables
if (!FactorClamped()) {
idLib::common->Printf("idLCP_Square::Solve: unbounded factorization failed\n");
return false;
}
SolveClamped(f, b.ToFloatPtr());
// if there are no bounded variables we are done
if (numUnbounded == m.GetNumRows()) {
o_x = f; // the vector is not permuted
return true;
}
}
#ifdef IGNORE_UNSATISFIABLE_VARIABLES
int numIgnored = 0;
#endif
// allocate for delta force and delta acceleration
delta_f.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
delta_a.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
// solve for bounded variables
failed = NULL;
for (i = numUnbounded; i < m.GetNumRows(); i++) {
// once we hit the box start index we can initialize the low and high boundaries of the variables using the box index
if (i == boxStartIndex) {
for (j = 0; j < boxStartIndex; j++) {
o_x[permuted[j]] = f[j];
}
for (j = boxStartIndex; j < m.GetNumRows(); j++) {
s = o_x[boxIndex[j]];
if (lo[j] != -idMath::INFINITY) {
lo[j] = - idMath::Fabs(lo[j] * s);
}
if (hi[j] != idMath::INFINITY) {
hi[j] = idMath::Fabs(hi[j] * s);
}
}
}
// calculate acceleration for current variable
SIMDProcessor->Dot(dot, rowPtrs[i], f.ToFloatPtr(), i);
a[i] = dot - b[i];
// if already at the low boundary
if (lo[i] >= -LCP_BOUND_EPSILON && a[i] >= -LCP_ACCEL_EPSILON) {
side[i] = -1;
continue;
}
// if already at the high boundary
if (hi[i] <= LCP_BOUND_EPSILON && a[i] <= LCP_ACCEL_EPSILON) {
side[i] = 1;
continue;
}
// if inside the clamped region
if (idMath::Fabs(a[i]) <= LCP_ACCEL_EPSILON) {
side[i] = 0;
AddClamped(i);
continue;
}
// drive the current variable into a valid region
for (n = 0; n < maxIterations; n++) {
// direction to move
if (a[i] <= 0.0f) {
dir = 1.0f;
} else {
dir = -1.0f;
}
// calculate force delta
CalcForceDelta(i, dir);
// calculate acceleration delta: delta_a = m * delta_f;
CalcAccelDelta(i);
// maximum step we can take
GetMaxStep(i, dir, maxStep, limit, limitSide);
if (maxStep <= 0.0f) {
#ifdef IGNORE_UNSATISFIABLE_VARIABLES
// ignore the current variable completely
lo[i] = hi[i] = 0.0f;
f[i] = 0.0f;
side[i] = -1;
numIgnored++;
#else
failed = va("invalid step size %.4f", maxStep);
#endif
break;
}
// change force
ChangeForce(i, maxStep);
// change acceleration
ChangeAccel(i, maxStep);
// clamp/unclamp the variable that limited this step
side[limit] = limitSide;
switch (limitSide) {
case 0: {
a[limit] = 0.0f;
AddClamped(limit);
break;
}
case -1: {
f[limit] = lo[limit];
if (limit != i) {
RemoveClamped(limit);
}
break;
}
case 1: {
f[limit] = hi[limit];
if (limit != i) {
RemoveClamped(limit);
}
break;
}
}
// if the current variable limited the step we can continue with the next variable
if (limit == i) {
break;
}
}
if (n >= maxIterations) {
failed = va("max iterations %d", maxIterations);
break;
}
if (failed) {
break;
}
}
#ifdef IGNORE_UNSATISFIABLE_VARIABLES
if (numIgnored) {
if (lcp_showFailures.GetBool()) {
idLib::common->Printf("idLCP_Symmetric::Solve: %d of %d bounded variables ignored\n", numIgnored, m.GetNumRows() - numUnbounded);
}
}
#endif
// if failed clear remaining forces
if (failed) {
if (lcp_showFailures.GetBool()) {
idLib::common->Printf("idLCP_Square::Solve: %s (%d of %d bounded variables ignored)\n", failed, m.GetNumRows() - i, m.GetNumRows() - numUnbounded);
}
for (j = i; j < m.GetNumRows(); j++) {
f[j] = 0.0f;
}
}
#if defined(_DEBUG) && 0
if (!failed) {
// test whether or not the solution satisfies the complementarity conditions
for (i = 0; i < m.GetNumRows(); i++) {
a[i] = -b[i];
for (j = 0; j < m.GetNumRows(); j++) {
a[i] += rowPtrs[i][j] * f[j];
}
if (f[i] == lo[i]) {
if (lo[i] != hi[i] && a[i] < -LCP_ACCEL_EPSILON) {
int bah1 = 1;
}
} else if (f[i] == hi[i]) {
if (lo[i] != hi[i] && a[i] > LCP_ACCEL_EPSILON) {
int bah2 = 1;
}
} else if (f[i] < lo[i] || f[i] > hi[i] || idMath::Fabs(a[i]) > 1.0f) {
int bah3 = 1;
}
}
}
#endif
// unpermute result
for (i = 0; i < f.GetSize(); i++) {
o_x[permuted[i]] = f[i];
}
// unpermute original matrix
for (i = 0; i < m.GetNumRows(); i++) {
for (j = 0; j < m.GetNumRows(); j++) {
if (permuted[j] == i) {
break;
}
}
if (i != j) {
m.SwapColumns(i, j);
idSwap(permuted[i], permuted[j]);
}
}
return true;
}
