float y2 = joint.getBody2().getPosition().getY();
g.drawLine((int) x1,(int) y1,(int) x2,(int) y2);
}
if(j instanceof SlideJoint){
SlideJoint joint = (SlideJoint) j;
Body b1 = joint.getBody1();
Body b2 = joint.getBody2();
Matrix2f R1 = new Matrix2f(b1.getRotation());
Matrix2f R2 = new Matrix2f(b2.getRotation());
ROVector2f x1 = b1.getPosition();
Vector2f p1 = MathUtil.mul(R1,joint.getAnchor1());
p1.add(x1);
ROVector2f x2 = b2.getPosition();
Vector2f p2 = MathUtil.mul(R2,joint.getAnchor2());
p2.add(x2);
Vector2f im = new Vector2f(p2);
im.sub(p1);
im.normalise();
g.setColor(Color.red);
g.drawLine((int)p1.x,(int)p1.y,(int)(p1.x+im.x*joint.getMinDistance()),(int)(p1.y+im.y*joint.getMinDistance()));
g.setColor(Color.blue);
g.drawLine((int)(p1.x+im.x*joint.getMinDistance()),(int)(p1.y+im.y*joint.getMinDistance()),(int)(p1.x+im.x*joint.getMaxDistance()),(int)(p1.y+im.y*joint.getMaxDistance()));
}
if(j instanceof AngleJoint){
AngleJoint angleJoint = (AngleJoint)j;
Body b1 = angleJoint.getBody1();
Body b2 = angleJoint.getBody2();
float RA = j.getBody1().getRotation() + angleJoint.getRotateA();
float RB = j.getBody1().getRotation() + angleJoint.getRotateB();
Vector2f VA = new Vector2f((float) Math.cos(RA), (float) Math.sin(RA));
Vector2f VB = new Vector2f((float) Math.cos(RB), (float) Math.sin(RB));
Matrix2f R1 = new Matrix2f(b1.getRotation());
Matrix2f R2 = new Matrix2f(b2.getRotation());
ROVector2f x1 = b1.getPosition();
Vector2f p1 = MathUtil.mul(R1,angleJoint.getAnchor1());
p1.add(x1);
ROVector2f x2 = b2.getPosition();
Vector2f p2 = MathUtil.mul(R2,angleJoint.getAnchor2());
p2.add(x2);
g.setColor(Color.red);
g.drawLine((int)p1.x,(int)p1.y,(int)(p1.x+VA.x*20),(int)(p1.y+VA.y*20));
g.drawLine((int)p1.x,(int)p1.y,(int)(p1.x+VB.x*20),(int)(p1.y+VB.y*20));
}
if (j instanceof BasicJoint) {
BasicJoint joint = (BasicJoint) j;
Body b1 = joint.getBody1();
Body b2 = joint.getBody2();
Matrix2f R1 = new Matrix2f(b1.getRotation());
Matrix2f R2 = new Matrix2f(b2.getRotation());
ROVector2f x1 = b1.getPosition();
Vector2f p1 = MathUtil.mul(R1,joint.getLocalAnchor1());
p1.add(x1);
ROVector2f x2 = b2.getPosition();
Vector2f p2 = MathUtil.mul(R2,joint.getLocalAnchor2());
p2.add(x2);
g.setColor(Color.red);
g.drawLine((int) x1.getX(), (int) x1.getY(), (int) p1.x, (int) p1.y);
g.drawLine((int) p1.x, (int) p1.y, (int) x2.getX(), (int) x2.getY());
g.drawLine((int) x2.getX(), (int) x2.getY(), (int) p2.x, (int) p2.y);
g.drawLine((int) p2.x, (int) p2.y, (int) x1.getX(), (int) x1.getY());
}
if(j instanceof DistanceJoint){
DistanceJoint joint = (DistanceJoint) j;
Body b1 = joint.getBody1();
Body b2 = joint.getBody2();
Matrix2f R1 = new Matrix2f(b1.getRotation());
Matrix2f R2 = new Matrix2f(b2.getRotation());
ROVector2f x1 = b1.getPosition();
Vector2f p1 = MathUtil.mul(R1,joint.getAnchor1());
p1.add(x1);
ROVector2f x2 = b2.getPosition();
Vector2f p2 = MathUtil.mul(R2,joint.getAnchor2());
p2.add(x2);
g.setColor(Color.red);
g.drawLine((int) p1.getX(), (int) p1.getY(), (int) p2.x, (int) p2.y);
}
if (j instanceof SpringJoint) {
SpringJoint joint = (SpringJoint) j;
Body b1 = joint.getBody1();
Body b2 = joint.getBody2();
Matrix2f R1 = new Matrix2f(b1.getRotation());
Matrix2f R2 = new Matrix2f(b2.getRotation());
ROVector2f x1 = b1.getPosition();
Vector2f p1 = MathUtil.mul(R1,joint.getLocalAnchor1());
p1.add(x1);
ROVector2f x2 = b2.getPosition();
Vector2f p2 = MathUtil.mul(R2,joint.getLocalAnchor2());
p2.add(x2);
g.setColor(Color.red);
g.drawLine((int) x1.getX(), (int) x1.getY(), (int) p1.x, (int) p1.y);
g.drawLine((int) p1.x, (int) p1.y, (int) p2.getX(), (int) p2.getY());