public int AddColliderContactConstraint(ColliderContactConstraint ccc) { var index = cccCount; cccs[index] = ccc; cccCount++; return(index); }
public static void Solve(ColliderContactConstraint c) { var constraint = math.dot(c.normal, c.sphere.predicted) - c.sphere.radius; if (constraint >= 0) { return; } c.sphere.position -= c.normal * constraint; c.sphere.predicted -= c.normal * constraint; }
void FixedUpdate() { var dt = Time.fixedDeltaTime; // apply forward euler and predict positions for (var i = 0; i < sphereCount; i++) { var s = spheres[i]; s.position = s.transform.position; s.velocity = s.velocity * Damping + Gravity * dt; s.predicted = s.position + s.velocity * dt; var delta = s.predicted - s.position; var direction = math.normalize(delta); var distance = math.length(delta); var collisionCount = Physics.SphereCastNonAlloc(s.position, s.radius, direction, collisions, distance); for (var c = 0; c < collisionCount; c++) { // float3 pContact = collisions[c].point; // float3 displacement = pContact - s.position; // TODO: should calculate the contact time and velocity // float3 vContact = math.sqrt(s.velocity * s.velocity + 2 * Gravity * displacement); // vContact *= math.dot(vContact, s.velocity) < 0 ? -1 : 1; var nccc = new ColliderContactConstraint { sphere = s, normal = collisions[c].normal, point = collisions[c].point, velocity = s.velocity, elasticity = collisions[c].collider.material.bounciness, friction = collisions[c].collider.material.dynamicFriction }; AddColliderContactConstraint(nccc); } } // iteratively apply constraints for (var k = 0; k < iterationCount; k++) { for (var i = 0; i < cccCount; i++) { ColliderContactConstraint.Solve(cccs[i]); } } // update velocity and position for (var i = 0; i < sphereCount; i++) { var isIdle = math.lengthSquared(spheres[i].velocity) < IdleThreshold; spheres[i].velocity = isIdle ? new float3(0) : (spheres[i].predicted - spheres[i].position) / dt; spheres[i].position = spheres[i].predicted; spheres[i].transform.position = spheres[i].position; } for (var i = 0; i < cccCount; i++) { var mvn = math.dot(cccs[i].velocity, cccs[i].normal); var vn = cccs[i].normal * mvn; var vt = cccs[i].velocity - vn; cccs[i].sphere.velocity = vt * cccs[i].friction - vn * cccs[i].elasticity; } // flush the collisions cccCount = 0; }