Verlet :: collision math / spring

Below is the code to do the actual collisions, verlet style. You might scratch your head and wonder how such basic math will handle all your physics. In reality it simply won't: it will only get more or less realistic when you start connecting lots of particles with lots of springs to create a representation of physical bodies.

With physics, it's all about how you structure your data. You don't want to collide everything against everything. How to design and optimize a spatial datastructure is left as an exercise to the reader. Such code is too big to post.

public class VerletMath
{
   // plane <--> sphere

   public static final boolean collides(VerletPlane a, VerletSphere b)
   {
      float dx = b.particle.now.x - a.nx * a.d;
      float dy = b.particle.now.y - a.ny * a.d;
      float dz = b.particle.now.z - a.nz * a.d;

      return ((a.nx * dx) + (a.ny * dy) + (a.nz * dz) - b.radius) < 0.0f;
   }

   public static final float collide(VerletPlane a, VerletSphere b)
   {
      float bx = b.particle.now.x;
      float by = b.particle.now.y;
      float bz = b.particle.now.z;
      float bd = b.radius;

      float dx = bx - a.nx * a.d;
      float dy = by - a.ny * a.d;
      float dz = bz - a.nz * a.d;

      float dst = (a.nx * dx) + (a.ny * dy) + (a.nz * dz) - bd;
      if (dst >= 0.0f)
         return 0.0f;

      // impl true bounce, using speed
      // push out along normal of plane

      b.particle.now.x = bx - dst * a.nx;
      b.particle.now.y = by - dst * a.ny;
      b.particle.now.z = bz - dst * a.nz;

      return -dst;
   }

   // sphere <--> sphere

   public static final void collide(VerletSphere target, Bag<VerletSphere> all)
   {
      int size = all.size();

      for (int i = 0; i < size; i++)
      {
         VerletSphere sphere = all.get(i);

         if (VerletMath.collides(sphere, target))
         {
            VerletMath.collide(sphere, target);
         }
      }
   }

   public static final boolean collides(VerletSphere a, VerletSphere b)
   {
      Vec3 va = a.particle.now;
      Vec3 vb = b.particle.now;
      float d = a.radius + b.radius;

      float x = va.x - vb.x;
      float y = va.y - vb.y;
      float z = va.z - vb.z;

      return (x * x + y * y + z * z) < (d * d);
   }

   public static final float collide(VerletSphere a, VerletSphere b)
   {
      Vec3 anow = a.particle.now;
      Vec3 bnow = b.particle.now;

      float ax = anow.x;
      float ay = anow.y;
      float az = anow.z;
      float aiw = a.particle.invWeight;

      float bx = bnow.x;
      float by = bnow.y;
      float bz = bnow.z;
      float biw = b.particle.invWeight;

      float dx = bx - ax;
      float dy = by - ay;
      float dz = bz - az;
      float d2 = dx * dx + dy * dy + dz * dz;

      if (d2 <= ulp_zero)
      {
         // sharing position, oh oh!
         // big problem! if we collide
         // it, it will explode 
         return 0.0f;
      }

      float dMin = a.radius + b.radius;
      if (d2 > (dMin * dMin))
         return 0.0f;

      // apply spring -> push out of eachother

      //final float tension = 1.0f;
      float d = (float) Math.sqrt(d2);
      float f = (d - dMin) / dMin * 0.5f;//* tension;

      float f1 = f * aiw / (aiw + biw);
      anow.x = ax + dx * f1;
      anow.y = ay + dy * f1;
      anow.z = az + dz * f1;

      float f2 = f * biw / (aiw + biw);
      bnow.x = bx - dx * f2;
      bnow.y = by - dy * f2;
      bnow.z = bz - dz * f2;

      return (dMin - d);
   }

   private static final float ulp_zero = Math.ulp(0.0f);
}

public class VerletSpring
{
   public static final int FIXED_LENGTH = 0;
   public static final int MIN_LENGTH   = 1;
   public static final int MAX_LENGTH   = 2;

   //

   public final VerletParticle a, b;

   public float                len, stf;
   public int                  how;

   public VerletSpring(VerletParticle a, VerletParticle b)
   {
      this.a = a;
      this.b = b;
   }

   //

   public final float setCurrentDistanceAsLength()
   {
      float dx = b.now.x - a.now.x;
      float dy = b.now.y - a.now.y;
      float dz = b.now.z - a.now.z;
      float d = (float) Math.sqrt(dx * dx + dy * dy + dz * dz);

      this.len = d;

      return d;
   }

   public final float tick()
   {
      float ax = a.now.x;
      float ay = a.now.y;
      float az = a.now.z;

      float bx = b.now.x;
      float by = b.now.y;
      float bz = b.now.z;

      float dx = ax - bx;
      float dy = ay - by;
      float dz = az - bz;
      float dist = (float) Math.sqrt(dx * dx + dy * dy + dz * dz);

      if (how == MIN_LENGTH)
      {
         if (dist > len)
            return 0.0f;
      }
      else if (how == MAX_LENGTH)
      {
         if (dist < len)
            return 0.0f;
      }

      float tension = (len - dist) / dist;
      float force = tension * this.stf;

      float aw = a.invWeight;
      float bw = b.invWeight;

      float f1 = force * aw / (aw + bw);
      float f2 = force * bw / (aw + bw);

      a.now.x = ax + dx * f1;
      a.now.y = ay + dy * f1;
      a.now.z = az + dz * f1;

      b.now.x = bx - dx * f2;
      b.now.y = by - dy * f2;
      b.now.z = bz - dz * f2;

      return tension;
   }
}