p2 heightfield interpolation

circleHeightfield.js

// Overwrite p2.Narrowphase.prototype.circleHeightfield
// Solves circle / heightfield collision with interpolated curve
// e.g. http://jsbin.com/mifixo/9/

/*jshint camelcase:false, maxparams: 15, maxcomplexity:15*/
var vec2 = p2.vec2;
var add = vec2.add;
var sub = vec2.sub;

var circleHeightfield_candidate = vec2.create();
var circleHeightfield_dist = vec2.create();
var circleHeightfield_v0 = vec2.create();
var circleHeightfield_v1 = vec2.create();
var circleHeightfield_minCandidate = vec2.create();
var circleHeightfield_worldNormal = vec2.create();
var circleHeightfield_minCandidateNormal = vec2.create();

function clamp(min, max, v) {
	return Math.max(Math.min(v, max), min);
}

function lerp(a, b, offset) {
	return (b - a) * offset + a;
}

p2.Narrowphase.prototype[p2.Shape.CIRCLE | p2.Shape.HEIGHTFIELD] =
p2.Narrowphase.prototype.circleHeightfield = function (
		circleBody, circleShape, circlePos, circleAngle,
		hfBody, hfShape, hfPos, hfAngle,
		justTest, radius) {

	radius = radius || circleShape.radius;

	var data = hfShape.data;
	var w = hfShape.elementWidth;

	var dist = circleHeightfield_dist;
	var candidate = circleHeightfield_candidate;
	var minCandidate = circleHeightfield_minCandidate;
	var minCandidateNormal = circleHeightfield_minCandidateNormal;
	var worldNormal = circleHeightfield_worldNormal;

	var v0 = circleHeightfield_v0;
	var v1 = circleHeightfield_v1;

	var intersectX = circlePos[0] - hfPos[0];

	var iMax = data.length - 1;
	var iB = clamp(0, iMax, Math.round(intersectX / w));
	var iA = clamp(0, iMax, iB - 1);
	var iC = clamp(0, iMax, iB + 1);

	var localOffset = intersectX % w / w - 0.5;
	if (localOffset < 0) { localOffset += 1; }

	var hA = lerp(data[iA], data[iB], localOffset);
	var hC = lerp(data[iB], data[iC], localOffset);
	// var hB = lerp(hA, hC, 0.5);

	var found = false;

	// Get points
	vec2.set(v0, intersectX - w * 0.5, hA);
	vec2.set(v1, intersectX + w * 0.5, hC);
	vec2.add(v0, v0, hfPos);
	vec2.add(v1, v1, hfPos);

	// Get normal
	vec2.sub(worldNormal, v1, v0);
	vec2.rotate(worldNormal, worldNormal, Math.PI/2);
	vec2.normalize(worldNormal,worldNormal);

	// Get point on circle, closest to the edge
	vec2.scale(candidate, worldNormal, -radius);
	vec2.add(candidate, candidate, circlePos);

	// Distance from v0 to the candidate point
	vec2.sub(dist, candidate, v0);

	// Check if it is in the element "stick"
	var d = vec2.dot(dist, worldNormal);
	if (candidate[0] >= v0[0] && candidate[0] < v1[0] && d <= 0) {

		if (justTest) {
			return true;
		}

		found = true;

		// Store the candidate point, projected to the edge
		vec2.scale(dist, worldNormal, -d);
		vec2.add(minCandidate, candidate, dist);
		vec2.copy(minCandidateNormal, worldNormal);

		var c = this.createContactEquation(hfBody, circleBody, hfShape, circleShape);

		// Normal is out of the heightfield
		vec2.copy(c.normalA, minCandidateNormal);

		// Vector from circle to heightfield
		vec2.scale(c.contactPointB, c.normalA, -radius);
		add(c.contactPointB, c.contactPointB, circlePos);
		sub(c.contactPointB, c.contactPointB, circleBody.position);

		vec2.copy(c.contactPointA, minCandidate);
		vec2.sub(c.contactPointA, c.contactPointA, hfBody.position);

		this.contactEquations.push(c);

		if (this.enableFriction) {
			this.frictionEquations.push(this.createFrictionFromContact(c));
		}
	}

	return found ? 1 : 0;
};