public static void processCollision(RigidBody rigidBody1, Vector3d point1,
			RigidBody rigidBody2, Vector3d point2, Vector3d relativeVelocity,
			Vector3d normal, Matrix3d k, double restitution, double timeStep) {
		Vector3d impulse = new Vector3d();

		// Coefficient of friction
		double friction = Math.max(rigidBody1.getFriction(),
				rigidBody2.getFriction());

		// Inverse of K
		Matrix3d kInverse = new Matrix3d(k);
		kInverse.invert();

		// Static friction impulse calculation
		Vector3d deltaURel = new Vector3d(normal);
		deltaURel.scale(-restitution * relativeVelocity.dot(normal));
		deltaURel.scaleAdd(-1, relativeVelocity, deltaURel);
		Vector3d row0 = new Vector3d();
		Vector3d row1 = new Vector3d();
		Vector3d row2 = new Vector3d();
		kInverse.getRow(0, row0);
		kInverse.getRow(1, row1);
		kInverse.getRow(2, row2);
		impulse.set(row0.dot(deltaURel), row1.dot(deltaURel),
				row2.dot(deltaURel));

		Vector3d impulseTangential = new Vector3d(normal);
		impulseTangential.scaleAdd(-impulse.dot(normal), impulse);

		if (impulseTangential.length() <= friction * impulse.dot(normal)) {
			// Static friction<

			if (rigidBody2.dynamicsType == RigidBody.DYNAMIC) {
				rigidBody2.applyImpulse(impulse, point2, timeStep);
			}
			if (rigidBody1.dynamicsType == RigidBody.DYNAMIC) {
				impulse.scale(-1);
				rigidBody1.applyImpulse(impulse, point1, timeStep);
			}
		} else {
			// Kinetic friction
			// Re-using impulseTangential
			impulseTangential.set(normal);
			impulseTangential.scaleAdd(-relativeVelocity.dot(normal),
					relativeVelocity);
			impulseTangential.normalize();

			Vector3d frictionSubtractedNormal = new Vector3d(impulseTangential);
			frictionSubtractedNormal.scale(-friction);
			frictionSubtractedNormal.add(normal);

			Vector3d col0 = new Vector3d();
			Vector3d col1 = new Vector3d();
			Vector3d col2 = new Vector3d();
			k.getColumn(0, col0);
			k.getColumn(1, col1);
			k.getColumn(2, col2);
			double impulseNormal = -(restitution + 1)
					* relativeVelocity.dot(normal)
					/ new Vector3d(normal.dot(col0), normal.dot(col1),
							normal.dot(col2)).dot(frictionSubtractedNormal);
			impulse.set(impulseTangential);
			impulse.scale(-friction * impulseNormal);
			impulse.scaleAdd(impulseNormal, normal, impulse);

			if (rigidBody2.dynamicsType == RigidBody.DYNAMIC) {
				rigidBody2.applyImpulse(impulse, point2, timeStep);
			}
			if (rigidBody1.dynamicsType == RigidBody.DYNAMIC) {
				impulse.scale(-1);
				rigidBody1.applyImpulse(impulse, point1, timeStep);
			}
		}
	}