Saturday, March 20, 2010

Obstacle in range

Used http://stackoverflow.com/questions/1073336/circle-line-collision-detection article as reference for line-circle intersection.

Following code extracts the part of line that is inside a circle:


 public static VectorLine2D calcLineCircleIntersection(VectorLine2D line, double cx, double cy, double cr)
 {
  // vector from center of the circle to one of line's edges:
  Vector2D f = new Vector2D(line.a.x-cx, line.a.y-cy);
  double a = line.d.dot(line.d) ;
  double b = 2 * f.dot(line.d) ;
  double c = f.dot(f) - cr*cr ;

  double discriminant = b*b-4*a*c;
  if( discriminant < 0 )
   return null;

    // ray didn't totally miss sphere,
    // so there is a solution to
    // the equation.
    discriminant = Math.sqrt( discriminant );
    double m1 = (-b + discriminant)/(2*a);
    double m2 = (-b - discriminant)/(2*a);
    
    // for convenience:
    double t1 = Math.min(m1, m2);
    double t2 = Math.max(m1, m2);
    
    
    Vector2D p1 = null;
    Vector2D p2 = null;
  
    if( t1 >= 0 && t1 <= 1) // intersection point closest to line.a
    {
     p1 = new Vector2D(line.a.x+line.d.x*t1, line.a.y+line.d.y*t1);
     p2 = line.b; // if this is not true, p2 will be updated in next "if"
    }
    
    if( t2 >= 0 && t2 <= 1)
    {
     p2 = new Vector2D(line.a.x+line.d.x*t2, line.a.y+line.d.y*t2);
     if(p1 == null) // if p1 was not set before, this is always true:
      p1 = line.a;
    }
    
    if(t1 < 0 && t2 > 1) // line is totally inside the circle
    {
     p1 = line.a;
     p2 = line.b;
    }
    
    if(p1 == null || p2 == null)
     return null;
    
    return new VectorLine2D(p1, p2);
 }

Just for reference, VectorLine2D implementation:
package yarangi.math.algebra;

/** 
 * This line implementation hold both cartesian and vector representation of the line.
 * This allows faster calculations on the cost of additional memory space.
 * 
 * @author Dve Yarangi
 */
public class VectorLine2D 
{
 /** first point of the line */
 public final Vector2D a;
 
 /** second point of the line */
 public final Vector2D b;
 
 /** non-normalized line direction vector */
 public final Vector2D d;
 
 public VectorLine2D(Vector2D a, Vector2D b)
 {
  this.a = a;
  this.b = b;
  
  d = new Vector2D(b.x-a.x, b.y-a.y);
 }
 
 public VectorLine2D(double x1, double y1, double x2, double y2)
 {
  this(new Vector2D(x1, y1), new Vector2D(x2, y2));
  
 }
 
 public boolean equals(Object o)
 {
  if(!(o instanceof VectorLine2D))
   return false;
  VectorLine2D l = (VectorLine2D) o;
  
  return a.x == l.a.x && a.y == l.a.y
      && b.x == l.b.x && a.y == l.b.y;
 }
 
 public int hashCode()
 {
  return a.hashCode() + b.hashCode();
 }
}

No comments:

Post a Comment