Permalink
Find file
Fetching contributors…
Cannot retrieve contributors at this time
320 lines (301 sloc) 10.1 KB
package geometry
import (
"math"
)
// Intersection3DLineLine, sets z to the shortest line between a and b then
// returns 1.
func Intersection3DLineLine(a, b, z *Line3D) int {
// http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
pdx, pdy, pdz := a.P.X-b.P.X, a.P.Y-b.P.Y, a.P.Z-b.P.Z
d1343 := pdx*b.V.X + pdy*b.V.Y + pdz*b.V.Z
d4321 := b.V.X*a.V.X + b.V.Y*a.V.Y + b.V.Z*a.V.Z
d1321 := pdx*a.V.X + pdy*a.V.Y + pdz*a.V.Z
d4343 := b.V.X*b.V.X + b.V.Y*b.V.Y + b.V.Z*b.V.Z
d2121 := a.V.X*a.V.X + a.V.Y*a.V.Y + a.V.Z*a.V.Z
mua := (d1343*d4321 - d1321*d4343) / (d2121*d4343 - d4321*d4321)
mub := (d1343 + mua*d4321) / d4343
z.P.X = a.V.X*mua + a.P.X
z.P.Y = a.V.Y*mua + a.P.Y
z.P.Z = a.V.Z*mua + a.P.Z
z.V.X = (b.V.X*mub + b.P.X) - z.P.X
z.V.Y = (b.V.Y*mub + b.P.Y) - z.P.Y
z.V.Z = (b.V.Z*mub + b.P.Z) - z.P.Z
return 1
}
// Intersection3DLineSphere sets y and z to the possible intersections of line a
// with sphere b and returns the number of intersections.
//
// Possible return values are:
// 0 for no intersections, y and z are untouched.
// 1 if the line is a tangent of the sphere, y is set and z is untouched.
// 2 if the line intersects the sphere, y and z are set to the two intersection
// points.
func Intersection3DLineSphere(a *Line3D, b *Sphere, y, z *Vector3D) int {
// http://paulbourke.net/geometry/circlesphere/index.html
aa := a.V.X*a.V.X + a.V.Y*a.V.Y + a.V.Z*a.V.Z
bb := 2 * (a.V.X*(a.P.X-b.C.X) + a.V.Y*(a.P.Y-b.C.Y) + a.V.Z*(a.P.Z-b.C.Z))
cc := b.C.X*b.C.X + b.C.Y*b.C.Y + b.C.Z*b.C.Z + a.P.X*a.P.X + a.P.Y*a.P.Y + a.P.Z*a.P.Z
cc -= 2 * (b.C.X*a.P.X + b.C.Y*a.P.Y + b.C.Z*a.P.Z)
cc -= b.R * b.R
rr := bb*bb - 4*aa*cc
if rr < 0 {
return 0
}
if rr == 0 {
u := -bb / (2 * aa)
y.X = a.P.X + u*a.V.X
y.Y = a.P.Y + u*a.V.Y
y.Z = a.P.Z + u*a.V.Z
return 1
}
rr = math.Sqrt(rr)
aa = 1 / (2 * aa)
u := (-bb + rr) * aa
y.X = a.P.X + u*a.V.X
y.Y = a.P.Y + u*a.V.Y
y.Z = a.P.Z + u*a.V.Z
u = (-bb - rr) * aa
z.X = a.P.X + u*a.V.X
z.Y = a.P.Y + u*a.V.Y
z.Z = a.P.Z + u*a.V.Z
return 2
}
// Intersection3DLineSegmentLineSegment determines the shortest line segment
// between a and b, then returns 1.
func Intersection3DLineSegmentLineSegment(a, b, z *Line3D) int {
// http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
pdx, pdy, pdz := a.P.X-b.P.X, a.P.Y-b.P.Y, a.P.Z-b.P.Z
d1343 := pdx*b.V.X + pdy*b.V.Y + pdz*b.V.Z
d4321 := b.V.X*a.V.X + b.V.Y*a.V.Y + b.V.Z*a.V.Z
d1321 := pdx*a.V.X + pdy*a.V.Y + pdz*a.V.Z
d4343 := b.V.X*b.V.X + b.V.Y*b.V.Y + b.V.Z*b.V.Z
d2121 := a.V.X*a.V.X + a.V.Y*a.V.Y + a.V.Z*a.V.Z
mua := (d1343*d4321 - d1321*d4343) / (d2121*d4343 - d4321*d4321)
mub := (d1343 + mua*d4321) / d4343
if mua < 0 {
z.P.X = a.P.X
z.P.Y = a.P.Y
z.P.Z = a.P.Z
} else if mua > 1 {
z.P.X = a.P.X + a.V.X
z.P.Y = a.P.Y + a.V.Y
z.P.Z = a.P.Z + a.V.Z
} else {
z.P.X = a.V.X*mua + a.P.X
z.P.Y = a.V.Y*mua + a.P.Y
z.P.Z = a.V.Z*mua + a.P.Z
}
if mub < 0 {
z.V.X = b.P.X - z.P.X
z.V.Y = b.P.Y - z.P.Y
z.V.Z = b.P.Z - z.P.Z
} else if mub > 1 {
z.V.X = (b.P.X + b.V.X) - z.P.X
z.V.Y = (b.P.Y + b.V.Y) - z.P.Y
z.V.Z = (b.P.Z + b.V.Z) - z.P.Z
} else {
z.V.X = (b.V.X*mub + b.P.X) - z.P.X
z.V.Y = (b.V.Y*mub + b.P.Y) - z.P.Y
z.V.Z = (b.V.Z*mub + b.P.Z) - z.P.Z
}
return 1
}
// Intersection3DPlaneLine sets z to the intersection of plane a and line b,
// then returns 1.
func Intersection3DPlaneLine(a *Plane, b *Line3D, z *Vector3D) int {
// http://paulbourke.net/geometry/planeline/
u := (a.A*b.P.X + a.B*b.P.Y + a.C*b.P.Z + a.D) / (a.A*b.V.X + a.B*b.V.Y + a.C*b.V.Z)
z.X = b.P.X - u*b.V.X
z.Y = b.P.Y - u*b.V.Y
z.Z = b.P.Z - u*b.V.Z
return 1
}
// Intersection3DPlanePlane sets z to the intersection of planes a and b, then
// returns 1.
func Intersection3DPlanePlane(a, b *Plane, z *Line3D) int {
// http://paulbourke.net/geometry/planeplane/
n1n1 := a.A*a.A + a.B*a.B + a.C*a.C
n2n2 := b.A*b.A + b.B*b.B + b.C*b.C
n1n2 := a.A*b.A + a.B*b.B + a.C*b.C
d := 1 / (n1n1*n2n2 - n1n2*n1n2)
c1 := (b.D*n1n2 - a.D*n2n2) * d
c2 := (a.D*n1n2 - b.D*n1n1) * d
z.P.X = c1*a.A + c2*b.A
z.P.Y = c1*a.B + c2*b.B
z.P.Z = c1*a.C + c2*b.C
z.V.X = a.B*b.C - a.C*b.B
z.V.Y = a.C*b.A - a.A*b.C
z.V.Z = a.A*b.B - a.B*b.A
return 1
}
// Intersection3DPlanePlanePlane sets z to the intersection of planes a, b, and
// c, then returns 1.
func Intersection3DPlanePlanePlane(a, b, c *Plane, z *Vector3D) int {
// http://paulbourke.net/geometry/3planes/
n2n3x := b.B*c.C - b.C*c.B
n2n3y := b.C*c.A - b.A*c.C
n2n3z := b.A*c.B - b.B*c.A
n3n1x := c.B*a.C - c.C*a.B
n3n1y := c.C*a.A - c.A*a.C
n3n1z := c.A*a.B - c.B*a.A
n1n2x := a.B*b.C - a.C*b.B
n1n2y := a.C*b.A - a.A*b.C
n1n2z := a.A*b.B - a.B*b.A
d := -1 / (a.A*n2n3x + a.B*n2n3y + a.C*n2n3z)
z.X = (a.D*n2n3x + b.D*n3n1x + c.D*n1n2x) * d
z.Y = (a.D*n2n3y + b.D*n3n1y + c.D*n1n2y) * d
z.Z = (a.D*n2n3z + b.D*n3n1z + c.D*n1n2z) * d
return 1
}
// Intersection3DFuzzyPlaneLine sets z to the intersection of a plane and line,
// then returns the number of intersections.
//
// Possible return values are:
// -1 if the plane and line are coincident, z is untouched.
// 0 if the plane and line are parallel, z is untouched.
// 1 if an intersection occurs, z is set to the intersection point.
func Intersection3DFuzzyPlaneLine(a *Plane, b *Line3D, z *Vector3D) int {
// http://paulbourke.net/geometry/planeline/
dot2 := a.A*b.V.X + a.B*b.V.Y + a.C*b.V.Z
dot1 := a.A*b.P.X + a.B*b.P.Y + a.C*b.P.Z
if FuzzyEqual(dot2, 0) {
if FuzzyEqual(dot1, 0) {
return -1
}
return 0
}
u := (dot1 + a.D) / dot2
z.X = b.P.X - u*b.V.X
z.Y = b.P.Y - u*b.V.Y
z.Z = b.P.Z - u*b.V.Z
return 1
}
// Intersection3DFuzzyPlanePlane sets z to the intersection of planes a and b,
// then returns the number of intersections.
//
// Possible return values are:
// -1 if the planes are coincident, z is untouched.
// 0 if the planes are parallel, z is untouched.
// 1 if an intersection occurs, z is set to the intersection line.
func Intersection3DFuzzyPlanePlane(a, b *Plane, z *Line3D) int {
// http://paulbourke.net/geometry/planeplane/
cpx, cpy, cpz := a.B*b.C-a.C*b.B, a.C*b.A-a.A*b.C, a.A*b.B-a.B*b.A
n1n1 := a.A*a.A + a.B*a.B + a.C*a.C
n2n2 := b.A*b.A + b.B*b.B + b.C*b.C
if FuzzyEqual(cpx*cpx+cpy*cpy+cpz*cpz, 0) {
// TODO a.A or b.A almost zero
s := a.A / b.A
if s*a.D*b.D < 0 || !FuzzyEqual(a.B, s*b.B) || !FuzzyEqual(a.C, s*b.C) ||
!FuzzyEqual(b.D*b.D*n1n1, a.D*a.D*n2n2) {
return 0
}
return -1
}
n1n2 := a.A*b.A + a.B*b.B + a.C*b.C
d := 1 / (n1n1*n2n2 - n1n2*n1n2)
c1 := (b.D*n1n2 - a.D*n2n2) * d
c2 := (a.D*n1n2 - b.D*n1n1) * d
z.P.X = c1*a.A + c2*b.A
z.P.Y = c1*a.B + c2*b.B
z.P.Z = c1*a.C + c2*b.C
z.V.X = cpx
z.V.Y = cpy
z.V.Z = cpz
return 1
}
// Intersection3DFuzzyPlanePlanePlane sets z to the intersection of 3 planes,
// then returns the number of intersections.
//
// Possible return values are:
// -3 if two planes are parallel and the third intersects at two lines.
// -2 if all three planes intersect at a line.
// -1 if all three planes are coincident.
// 0 if all planes are parallel (two could be coincident), z is untouched.
// 1 if the planes intersect at a point, z is set to the intersection point.
func Intersection3DFuzzyPlanePlanePlane(a, b, c *Plane, z *Vector3D) int {
n1n1 := a.A*a.A + a.B*a.B + a.C*a.C
n2n2 := b.A*b.A + b.B*b.B + b.C*b.C
n3n3 := c.A*c.A + c.B*c.B + c.C*c.C
// use cross products to check if plane normals are equal
cpabx, cpaby, cpabz := a.B*b.C-a.C*b.B, a.C*b.A-a.A*b.C, a.A*b.B-a.B*b.A
n1n2d := FuzzyEqual(cpabx*cpabx+cpaby*cpaby+cpabz*cpabz, 0)
cpcax, cpcay, cpcaz := c.B*a.C-c.C*a.B, c.C*a.A-c.A*a.C, c.A*a.B-c.B*a.A
n3n1d := FuzzyEqual(cpcax*cpcax+cpcay*cpcay+cpcaz*cpcaz, 0)
// check if all planes are parallel
if n1n2d && n3n1d {
// check if all planes are coincident
if FuzzyEqual(b.D*b.D*n1n1, a.D*a.D*n2n2) &&
FuzzyEqual(c.D*c.D*n1n1, a.D*a.D*n3n3) {
return -1
} else {
return 0
}
}
cpbcx, cpbcy, cpbcz := b.B*c.C-b.C*c.B, b.C*c.A-b.A*c.C, b.A*c.B-b.B*c.A
// check if lines from pair of plane intersections have the same direction
if ldx, ldy, ldz := cpaby*cpbcz-cpabz*cpbcy, cpabz*cpbcx-cpabx*cpbcz,
cpabx*cpbcy-cpaby*cpbcx; FuzzyEqual(ldx*ldx+ldy*ldy+ldz*ldz, 0) {
// get point on each line
n1n2 := a.A*b.A + a.B*b.B + a.C*b.C
n2n3 := b.A*c.A + b.B*c.B + b.C*c.C
d1 := 1 / (n1n1*n2n2 - n1n2*n1n2)
d2 := 1 / (n2n2*n3n3 - n2n3*n2n3)
c1 := (b.D*n1n2 - a.D*n2n2) * d1
c2 := (a.D*n1n2 - b.D*n1n1) * d1
c3 := (c.D*n2n3 - b.D*n3n3) * d2
c4 := (b.D*n2n3 - c.D*n2n2) * d2
p1x, p1y, p1z := c1*a.A+c2*b.A, c1*a.B+c2*b.B, c1*a.C+c2*b.C
p2x, p2y, p2z := c3*b.A+c4*c.A, c3*b.B+c4*c.B, c3*b.C+c4*c.C
// check if points lie on the third plane
if FuzzyEqual(p1x*c.A+p1y*c.B+p1z*c.C+c.D, 0) &&
FuzzyEqual(p2x*a.A+p2y*a.B+p2z*a.C+a.D, 0) {
return -2
}
}
// check for a pair of parallel planes resulting in 2 lines, all 3 parallel
// and 2 coincident have been caught already
if n1n2d || n3n1d || FuzzyEqual(cpbcx*cpbcx+cpbcy*cpbcy+cpbcz*cpbcz, 0) {
return -3
}
// having ruled out all degenerate cases calculate intersection
d := -1 / (a.A*cpbcx + a.B*cpbcy + a.C*cpbcz)
z.X = (a.D*cpbcx + b.D*cpcax + c.D*cpabx) * d
z.Y = (a.D*cpbcy + b.D*cpcay + c.D*cpaby) * d
z.Z = (a.D*cpbcz + b.D*cpcaz + c.D*cpabz) * d
return 1
}
// Intersection3DRaySphere sets z to the first intersection of ray a with sphere
// b and returns the number of intersections, either 1 or 0.
func Intersection3DRaySphere(a *Line3D, b *Sphere, z *Vector3D) int {
aa := a.V.X*a.V.X + a.V.Y*a.V.Y + a.V.Z*a.V.Z
bb := 2 * (a.V.X*(a.P.X-b.C.X) + a.V.Y*(a.P.Y-b.C.Y) + a.V.Z*(a.P.Z-b.C.Z))
cc := b.C.X*b.C.X + b.C.Y*b.C.Y + b.C.Z*b.C.Z + a.P.X*a.P.X + a.P.Y*a.P.Y + a.P.Z*a.P.Z
cc -= 2 * (b.C.X*a.P.X + b.C.Y*a.P.Y + b.C.Z*a.P.Z)
cc -= b.R * b.R
rr := bb*bb - 4*aa*cc
if rr < 0 {
return 0
}
if rr == 0 {
u := -bb / (2 * aa)
z.X = a.P.X + u*a.V.X
z.Y = a.P.Y + u*a.V.Y
z.Z = a.P.Z + u*a.V.Z
return 1
}
rr = math.Sqrt(rr)
aa = 1 / (2 * aa)
if u := (-bb + rr) * aa; u > 0 {
z.X = a.P.X + u*a.V.X
z.Y = a.P.Y + u*a.V.Y
z.Z = a.P.Z + u*a.V.Z
return 1
}
if u := (-bb - rr) * aa; u > 0 {
z.X = a.P.X + u*a.V.X
z.Y = a.P.Y + u*a.V.Y
z.Z = a.P.Z + u*a.V.Z
return 1
}
return 0
}