Exemple #1
0
/*
============
idBox::FromPoints

  Tight box for a collection of points.
============
*/
void idBox::FromPoints( const idVec3 *points, const int numPoints ) {
	int i;
	float invNumPoints, sumXX, sumXY, sumXZ, sumYY, sumYZ, sumZZ;
	idVec3 dir;
	idBounds bounds;
	idMatX eigenVectors;
	idVecX eigenValues;

	// compute mean of points
	center = points[0];
	for ( i = 1; i < numPoints; i++ ) {
		center += points[i];
	}
	invNumPoints = 1.0f / numPoints;
	center *= invNumPoints;

	// compute covariances of points
	sumXX = 0.0f; sumXY = 0.0f; sumXZ = 0.0f;
	sumYY = 0.0f; sumYZ = 0.0f; sumZZ = 0.0f;
	for ( i = 0; i < numPoints; i++ ) {
		dir = points[i] - center;
		sumXX += dir.x * dir.x;
		sumXY += dir.x * dir.y;
		sumXZ += dir.x * dir.z;
		sumYY += dir.y * dir.y;
		sumYZ += dir.y * dir.z;
		sumZZ += dir.z * dir.z;
	}
	sumXX *= invNumPoints;
	sumXY *= invNumPoints;
	sumXZ *= invNumPoints;
	sumYY *= invNumPoints;
	sumYZ *= invNumPoints;
	sumZZ *= invNumPoints;

	// compute eigenvectors for covariance matrix
	eigenValues.SetData( 3, VECX_ALLOCA( 3 ) );
	eigenVectors.SetData( 3, 3, MATX_ALLOCA( 3 * 3 ) );

	eigenVectors[0][0] = sumXX;
	eigenVectors[0][1] = sumXY;
	eigenVectors[0][2] = sumXZ;
	eigenVectors[1][0] = sumXY;
	eigenVectors[1][1] = sumYY;
	eigenVectors[1][2] = sumYZ;
	eigenVectors[2][0] = sumXZ;
	eigenVectors[2][1] = sumYZ;
	eigenVectors[2][2] = sumZZ;
	eigenVectors.Eigen_SolveSymmetric( eigenValues );
	eigenVectors.Eigen_SortIncreasing( eigenValues );

	axis[0][0] = eigenVectors[0][0];
	axis[0][1] = eigenVectors[0][1];
	axis[0][2] = eigenVectors[0][2];
	axis[1][0] = eigenVectors[1][0];
	axis[1][1] = eigenVectors[1][1];
	axis[1][2] = eigenVectors[1][2];
	axis[2][0] = eigenVectors[2][0];
	axis[2][1] = eigenVectors[2][1];
	axis[2][2] = eigenVectors[2][2];

	extents[0] = eigenValues[0];
	extents[1] = eigenValues[0];
	extents[2] = eigenValues[0];

	// refine by calculating the bounds of the points projected onto the axis and adjusting the center and extents
	bounds.Clear();
    for ( i = 0; i < numPoints; i++ ) {
		bounds.AddPoint( idVec3( points[i] * axis[0], points[i] * axis[1], points[i] * axis[2] ) );
    }
	center = ( bounds[0] + bounds[1] ) * 0.5f;
	extents = bounds[1] - center;
	center *= axis;
}
Exemple #2
0
/*
============
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;
}