how to remove bug ? (2d physics)

Janglee123

I am working on 2d Physics engine . here is my code ( https://github.com/Janglee123/four ) . sometime it behave unpredictable that you can also see and I have to figure it out why this happens . So how to find bug in my code . I know it take time but I want to complete my Physics Engine . Please help me.Thank you in advance.

blyk

please post the code here. That will be much easier... thanks

Janglee123

it contains 6 tabs so it is hard to past here and is there any problem with github you can download it easily just change folder name to "four". after that you can run it . thanks for reply

Janglee123

    Polygone p,q,r,t;
    Space s;
    Vec2 v1,v2,v3;
    float a;

    void setup(){

      s = new Space();

      p = s.Polygone(300,300,100,3);
      q = s.Polygone(100,100,50,4);
      //r = s.Polygone(200,20,10,4);
      //t = s.Polygone(20,20,10,3);

      p.velocity = new Vec2(-1,-1);
      q.velocity = new Vec2(1,1);


      size(500,500);
      frameRate(30);
    }

    void draw(){
      //println(frameCount,frameRate);
      background(200);

       s.Display();
      s.Update();
      s.FixedUpdate();


    }

void moveback(Polygone pi,Polygone pj){


  float vi = Vmag(pi.velocity);
  float vj = Vmag(pj.velocity);

  pi.velocity = Vunit(pi.velocity);
  pi.velocity = Vmult(pi.velocity,-1);
  pj.velocity = Vunit(pj.velocity);
  pj.velocity = Vmult(pj.velocity,-1);

  if(vi!=0)
  pi.omega /= -vi;
  if(vj!=0)
  pj.omega /= -vj;

  while(cdPolyPoly(pi.vertex,pj.vertex)){

    pi.Update();
    pj.Update();
  }

  pi.velocity = Vmult(pi.velocity,-vi);
  pj.velocity = Vmult(pj.velocity,-vj);

  pi.omega *= -vi;
  pj.omega *= -vj;
}

void PhyA(Polygone pi,Polygone pj){
  Vec2 v = Vsub(pi.velocity,pj.velocity);
 //ever point of pi
  for(int in = 0 ; in < pi.n ; in++){
    //every line of pj
    for(int jn1 = 0,jn2 = 1 ; jn1 <pj.n ; jn1++ , jn2++){
      if(jn2==pj.n)jn2=0;

        if(cdPointLine(pi.vertex[in], pj.vertex[jn1] , pj.vertex[jn2])){
          //pi.vertex[in] is cp and n will be perpendicular to line(pj.vertex[jn1] , pj.vertex[jn2])
          Vec2 ri = Vsub(pi.vertex[in],pi.center);
          Vec2 rj = Vsub(pi.vertex[in],pj.center);
          Vec2 n = Vunit(Vnormal(Vsub(pj.vertex[jn2],pj.vertex[jn1])));

          Vdraw(rj,pj.center);
          Vdraw(ri,pi.center);
          Vdraw(n,pi.vertex[in]);

          float J = -2*Vdot(v,n) / ( pi.massi + pj.massi +  sq(Vdot(n,ri))*pi.ineri + sq(Vdot(n,rj))*pj.ineri );

          pi.velocity = Vsum(pi.velocity,Vmult(n,J*pi.massi));
          pj.velocity = Vsub(pj.velocity,Vmult(n,J*pj.massi));
          pi.omega += J*Vdot(n,ri)*pi.ineri;
          pj.omega -= J*Vdot(n,rj)*pj.ineri;
          println(pi.omega,pj.omega);
       }

    }
  }
}

int pID =0;

//creat 2d polygone 
class Polygone{

  Vec2 center;
  float thita;

  Vec2[] vertex;

  Vec2 velocity, acceleration;
  float omega , alpha;

  float[] r , angle;

  float mass,massi , iner , ineri , area , density ;

  int n ; 

  float a , rn , rm;

  int ID;

  Polygone(float x , float y , float a , int n ){

    density = 1;
    rm = a/(2*sin(PI/n));
    rn = sqrt(rm*rm - a*a/4);
    this.n = n;

    area = n*a*rn/2;
    mass = density*area;
    massi = 1/mass;
    iner = mass*(a*a + 12*rn*rn)/24;

    ineri =  1/iner ;

    center = new Vec2(x,y);
    velocity = new Vec2();
    acceleration = new Vec2();

    vertex = new Vec2[n];
    angle = new float[n];
    r = new float[n];

    for(int i = 0 ;  i  < n ; i ++){

      angle[i] = 2*PI*i/n;
      r[i] = rm;
      vertex[i] = new Vec2(center.x + r[i]*cos(angle[i] + thita) , center.y + r[i]*sin(angle[i] +thita));
    }

    //ineri=0;
    //omega = random(0.1);
    ID = pID++;
  } 


  void Display(){

    beginShape();
     for(Vec2 v : vertex){

       vertex(v.x,v.y);

     }
     endShape(CLOSE);

  }


  void Update(){

    velocity = Vsum(velocity,acceleration);
    center = Vsum(velocity,center);

    omega += alpha;
    thita += omega;


    for(int i = 0 ; i < n ; i++)
      vertex[i] = new Vec2(center.x + r[i]*cos(angle[i] + thita),center.y + r[i]*sin(angle[i] + thita));

  }

}

class Space{

  ArrayList<Polygone> Rigidbody ;
  Polygone pi,pj;

  Space(){

    Rigidbody = new ArrayList<Polygone>();
  }

  void Display(){

    for(int i =0 ; i < Rigidbody.size() ; i++)
      Rigidbody.get(i).Display();   
  }

  void Update(){

    for(int i =0 ; i < Rigidbody.size() ; i++)
      Rigidbody.get(i).Update();
  }

  Polygone Polygone(float x,float y,float a,int n){

    Rigidbody.add(new Polygone(x,y,a,n));
    return Rigidbody.get(Rigidbody.size()-1);
  }

  void FixedUpdate(){

    for(int i = 0 ; i < Rigidbody.size() ; i++ ){

      pi = Rigidbody.get(i);

      cdBoundry(pi,width,height);
      for(int j = i+1 ; j < Rigidbody.size() ; j++){

        pj = Rigidbody.get(j);
        if(cdPolyPoly(pi.vertex,pj.vertex)){
         //move to contact point 
         moveback(pi,pj); 
         //cp is a pi's vertex        
         PhyA(pi,pj);
         //cp is a pj's vertex            
         PhyA(pj,pi);

        }


      }
    }

  }  


}

class Vec2{
  float x,y;

  Vec2(float x,float y){

    this.x = x;
    this.y = y;
  }

  Vec2(Vec2 v){

    x = v.x;
    y = v.y;
  }

  Vec2(){

    x=0;
    y=0;
  }

  Vec2(float x ,float y ,int i){
    this.x = random(x);
    this.y = random(y);
  }
}


Vec2 Vsum(Vec2 v1,Vec2 v2){
   // if(v2.x!= 0 )
    return new Vec2(v1.x + v2.x , v1.y + v2.y);

   // return new Vec2(v1.x,v1.y+v2.y);
}

Vec2 Vsub(Vec2 v1 , Vec2 v2){

  return new Vec2(v1.x - v2.x , v1.y - v2.y);
}

float Vdot(Vec2 v1,Vec2 v2){

  return v1.x*v2.x + v1.y*v2.y;  
}

float Vcross(Vec2 v1,Vec2 v2){

  return v1.x*v2.y - v1.y*v2.x;
}

float Vmag(Vec2 v){

  return sqrt(v.x*v.x + v.y*v.y);
}

Vec2 Vdiv(Vec2 v , float d){

  return new Vec2(v.x/d , v.y/d );
}

Vec2 Vmult(Vec2 v , float m){

  return new Vec2(v.x*m,v.y*m);
}


Vec2 Vrotate(Vec2 v,float a){

  return new Vec2(v.x*cos(a) - v.y*sin(a),v.x*sin(a) + v.y*cos(a));
}

Vec2 Vnormal(Vec2 v){

  return new Vec2(v.y,-v.x); 
}


Vec2 Vunit(Vec2 v){ 

  if(v.x==0&&v.y==0)
    return new Vec2(0,0);

  float m = Vmag(v);  
  return new Vec2(v.x/m,v.y/m);
}

void Vprint(Vec2 v){

  println("[",v.x,",",v.y,"]");
}

float Vdist(Vec2 v1 , Vec2 v2){

  return dist(v1.x,v1.y,v2.x,v2.y);
}

void Vdraw(Vec2 v1,Vec2 v2){

  Vec2 v = Vunit(v1);
  pushStyle();
  stroke(0);
  fill(0);
  line(v2.x,v2.y,v2.x + 10*v.x,v2.y+10*v.y);
  strokeWeight(5);
  point(v2.x + 10*v.x,v2.y+10*v.y);
  popStyle();
}

boolean cdPointPoly(Vec2 v,Vec2[] p){

  boolean c = false;

  //https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html
  for(int i = 0 , j =  1 ; i < p.length ; i++ , j++ ){

    if(j==p.length)j=0;

    if ( ((p[i].y>v.y) != (p[j].y>v.y)) && 
    (v.x < (p[j].x-p[i].x) * (v.y-p[i].y) / (p[j].y-p[i].y) + p[i].x) )
       c = !c;
   }

  return c;
}


boolean cdPointLine(Vec2 p, Vec2 l1,Vec2 l2){

  float d = Vdist(l1,l2);
  float d1 = Vdist(p,l1);
  float d2 = Vdist(p,l2);

  if(d1+d2 < d + 0.1 && d1+d2 > d - 0.1 ){
    return true;
  }

  return false;
}

boolean cdLineLine(Vec2 l11 ,Vec2 l12,Vec2 l21,Vec2 l22){

  //http://paulbourke.net/geometry/pointlineplane/
  float d = (l22.y - l21.y)*(l12.x - l11.x) - (l22.x-l21.x)*(l12.y - l11.y);
  float n1 = (l22.x - l21.x)*(l11.y - l21.y) - (l22.y - l21.y)*(l11.x -l21.x);
  float n2 = (l12.x - l11.x)*(l11.y - l21.y) - (l12.y - l11.y)*(l11.x - l21.x);  
  float ua = n1/d;
  float ub = n2/d;

  if(d!=0){
    if( ua<=1 && ua>=0 && ub<=1 && ub>=0 ) return true;
  }

  else{
    if(n1 == 0 && n2 == 0) return true;
  }

  return false;
}


boolean cdLinePoly(Vec2 l1 , Vec2 l2 ,Vec2[] p){

  for(int i = 0 , j = 1 ; i < p.length ; i++ , j++){
    if(j==p.length)j=0;

    if(cdLineLine(l1,l2,p[i],p[j])) return true;
  }

  return false;
}


boolean cdPolyPoly(Vec2 p1[],Vec2 p2[]){

  if(cdLinePoly(p1[0],p1[1],p2)) return true;

  for(int i = 0 ; i < p1.length ; i ++){
    if(cdPointPoly(p1[i],p2)) return true;
  }

  for(int i = 0 ; i < p2.length ; i ++){
    if(cdPointPoly(p2[i],p1)) return true;
  }

  return false;
}


void cdBoundry(Polygone p,int w , int h){
  float dx = 0, dy = 0 ;
  int xc = 0 , yc = 0 ;
  for(int i = 0 ; i < p.n ; i++){
    if(p.vertex[i].x>w){
      xc++;
      dx = p.vertex[i].x - w ;
    }
     if(p.vertex[i].x<0){
      dx = p.vertex[i].x ;
      xc++;
    }

    if(p.vertex[i].y>h){
      dy = p.vertex[i].y - h ;
      yc++;
    }
     if(p.vertex[i].y<0){
      dy = p.vertex[i].y ;
      yc++;
    }

  }

  if(xc>0){
    p.velocity.x*=-1;
    //p.velocity.x-=p.acceleration.x/sqrt(abs(dx));
    p.center.x -= dx;
    for(int i = 0 ; i < p.n ; i++){
      p.vertex[i].x-=dx;

    }
  }

  if(yc>0){
   p.center.y -=dy;
   p.velocity.y*=-1;
   //p.velocity.y-=p.acceleration.y/sqrt(abs(dy));
    for(int i = 0 ; i < p.n ; i++){
      p.vertex[i].y-=dy;

    }
  }

  if(xc>0||yc>0)p.omega*=-1;
}