## P3D.py (module 'P3D') Version 1.08

## Copyright (c) 2006 Bruce Vaughan, BV Detailing & Design, Inc.

## NOT FOR SALE. The software is provided "as is" without any warranty.

#############################################################################

"""

/// Define a plane in 3D space from three non-collinear points.

/// Determine if another point lies on the defined plane.

/// Define two additional planes and calculate the radius and center point of a circle connecting the points.

/// Rotate a point about an axis perpendicular to the defined plane (axis is defined by p1 and p1 + N_uv).

///

/// Developed by Bruce Vaughan, BV Detailing & Design, Inc. (September 2006)

/// URL: www.bvdetailing.com

///

///

///

/// Revision History:

///     Version 1.01 (10/18/06) -   Removed method 'pr_init()'

///                                 Add optional argument to '__init__()'

///     Version 1.02 (11/11/06) -   Rewrite class method PointRotate3D

///     Version 1.03 (11/15/06) -   Rewrite Plane3D

///                                 Removed lie_check2 since it was redundant

///     Version 1.04 (12/06/06) -   Optimize method chk_type

///     Version 1.05 (12/18/06) -   Base class 'object' for Plane3D

///     Version 1.06 (1/10/07) -    Add __repr__ method Plane3D

///     Version 1.07 (1/27/07) -    Add instance attributes self.p2 and self.p3

///     Version 1.08 (2/16/07) -    Rename __repr__ to __str__ and add new __repr__ method

///

/// References 'Equation of a plane', 'Intersection of three planes', 'Rotate A Point About An Arbitrary Axis (3D)' - Paul Bourke

"""

###########################################

from math import pi, sqrt, acos, cos, sin

from point import Point

from param import dim_print

from macrolib.fifDim import fifDim

from macrolib.PrintDict import formatDict

class Plane3D(object):

def cross_product(self, p1, p2):

return Point(p1.y*p2.z - p1.z*p2.y, p1.z*p2.x - p1.x*p2.z, p1.x*p2.y - p1.y*p2.x)

def dot_product(self, p1, p2):

return (p1.x*p2.x + p1.y*p2.y + p1.z*p2.z)

def plane_def(self, p1, p2, p3):

N = self.cross_product(p2-p1, p3-p1)

A = N.x

B = N.y

C = N.z

D = self.dot_product(-N, p1)

return N, A, B, C, D

def __init__(self, p1, p2, p3, theta1 = 0):

# make points global to class namespace

self.p1 = p1

self.p2 = p2

self.p3 = p3

def chk_type(p_list):

for p in p_list:

if not isinstance(p, type(Point(0,0,0))):

return False

return True

if chk_type([p1, p2, p3]):

"""

/// Define a plane from 3 non-collinear points

/// Ax + By + Cz + D = 0

/// The normal 'N' to the plane is the vector (A, B, C)

"""

self.N, A, B, C, self.D = self.plane_def(p1, p2, p3)

self.N_len = round(sqrt(A*A + B*B + C*C), 6)

if self.N_len > 0.0:

self.N_uv = Point(self.N.x/self.N_len, self.N.y/self.N_len, self.N.z/self.N_len)

else:

self.N_uv = Point(0.0, 0.0, 0.0)

"""

/// If vector N is the normal to the plane then all points 'p' on the plane satisfy the following:

/// N dot p = k where 'dot' is the dot product

/// N dot p = N.x*p.x + N.y*p.y + N.z*p.z

"""

self.k = round(self.dot_product(self.N, p1), 6)             # calculation of plane constant 'k'

self.k0 = round(self.dot_product(self.N_uv, p1), 6)         # displacement of the plane from the origin

"""

/// Determine vector e and unit vector e0 (p1 to p3)

/// Determine vector d and unit vector d0 (p1 to p2)

/// Determine location of point F, midpoint on vector d

/// Determine location of point G, midpoint on vector e

"""

e = p3 - p1

e_len = (sqrt(e.x**2 + e.y**2 + e.z**2))

if e_len > 0.0:

self.e0 = Point(e.x/e_len, e.y/e_len, e.z/e_len)

else:

self.e0 = Point(0.0, 0.0, 0.0)

d = p2 - p1

d_len = (sqrt(d.x**2 + d.y**2 + d.z**2))

if d_len > 0.0:

self.d0 = Point(d.x/d_len, d.y/d_len, d.z/d_len)

else:

self.d0 = Point(0.0, 0.0, 0.0)

self.F = Point(p1.x + (d.x/2), p1.y + (d.y/2), p1.z + (d.z/2))

self.G = Point(p1.x + (e.x/2), p1.y + (e.y/2), p1.z + (e.z/2))

# Make variables 'e' and 'd' available as attributes

self.e = e

self.d = d

# Calculate distance between points p1 and p2 = arc radius

self.Ra = p2.dist(p1)

"""

/// Calculate net angle between vectors d0 and e0 (Q)

/// Calculate point to point distance (pp) aalong the arc

"""

if abs(theta1) == pi:

self.Q = theta1

else:

self.Q = acos(round(self.dot_product(self.d0, self.e0), 12))   # radians

self.pp = abs(self.Q * self.Ra)

else:

raise TypeError, "All arguments passed to Plane3D must be <type 'point'>"

def __str__(self):

s1 = 'Instance of Class %s - Plane Definition Data' % (repr(self.__class__).split('.')[1][:-2])

s2 = '\n  Normal Unit Vector: %0.4f, %0.4f, %0.4f' % (self.N_uv.x,self.N_uv.y,self.N_uv.z)

s3 = '\n  Displacement from Model Origin: %0.4f' % (self.k0)

s4 = '\n\nPlane Origin and Unit Vector Points'

s5 = '\n  Origin point: %s, %s, %s' % (fifDim(self.p1.x),fifDim(self.p1.y),fifDim(self.p1.z))

s6 = '\n  Unit Vector d0: %0.4f, %0.4f, %0.4f' % (self.d0.x,self.d0.y,self.d0.z)

s7 = '\n  Unit Vector e0: %0.4f, %0.4f, %0.4f' % (self.e0.x,self.e0.y,self.e0.z)

return '%s%s%s%s%s%s%s' % (s1, s2, s3, s4, s5, s6, s7)

def __repr__(self):

return 'Plane3D(Point(%s), Point(%s), Point(%s), %0.6f)' % (self.p1, self.p2, self.p3, self.Q)

# Calculate points to define plane #2 and calculate N2 and d2

def plane_2(self):

p1 = self.G

p2 = self.G + self.N_uv

p3 = self.G + self.cross_product(self.e0, self.N_uv)

N, A, B, C, D = self.plane_def(p1, p2, p3)

d = round(sqrt(A*A + B*B + C*C), 6)

self.N2 = Point(A/d, B/d, C/d)

self.d2 = round(self.N2.x*p1.x + self.N2.y*p1.y + self.N2.z*p1.z, 6)

# Calculate points to define plane #3 and calculate N3 and d3

def plane_3(self):

p1 = self.F

p2 = self.F + self.N_uv

p3 = self.F + self.cross_product(self.d0, self.N_uv)

N, A, B, C, D = self.plane_def(p1, p2, p3)

d = round(sqrt(A*A + B*B + C*C), 6)

self.N3 = Point(A/d, B/d, C/d)

self.d3 = round(self.N3.x*p1.x + self.N3.y*p1.y + self.N3.z*p1.z, 6)

def three_pt_circle (self):

"""

/// The intersection of three planes is either a point, a line, or there is no intersection.

/// Three planes can be written as:

/// N1 dot p = d1

/// N2 dot p = d2

/// N3 dot p = d3

/// 'Nx' is the normal vector

/// 'p' is a point on the plane

/// 'dot' signifies the dot product of 'Nx' and 'p'

/// 'dx' = plane constant (displacement of the plane from the origin if Nx is a unit vector)

/// The intersection point of the three planes "M" is given by:

/// M = (d1*(N2 cross N3) + d2(N3 cross N1) + d3*(N1 cross N2)) / (N1 dot (N2 cross N3))

/// 'cross' indicates the cross product and 'dot' indicates the dot product

/// Calculate the center point of the circle (intersection point of three planes) and the radius.

/// Plane 1 is defined in __init__ method.

"""

# Define Plane 2

self.plane_2()

# Define Plane 3

self.plane_3()

N23 = self.cross_product(self.N2, self.N3)

N31 = self.cross_product(self.N3, self.N)

N12 = self.cross_product(self.N, self.N2)

NdN23 = round(self.dot_product(self.N, N23), 6)

numer = Point(self.k*N23.x, self.k*N23.y, self.k*N23.z) + (self.d2*N31.x, self.d2*N31.y, self.d2*N31.z) + \

(self.d3*N12.x, self.d3*N12.y, self.d3*N12.z)

if NdN23 != 0.0:

self.M = Point(numer.x/NdN23, numer.y/NdN23, numer.z/NdN23)

self.R = self.M.dist(self.p1)

else:

self.M = Point(0.0, 0.0, 0.0)

self.R = 0.0

"""

/// Rotate point p about a line passing through self.p1 and normal to the current plane by the angle 'theta' in radians

/// Return the new point

"""

def PointRotate3D(self, p, theta):

# Initialize point q

q = Point(0.0,0.0,0.0)

# Rotation axis unit vector

n = self.N_uv

# Translate so axis is at origin

p = p - self.p1

# Matrix common factors

c = cos(theta)

t = (1 - cos(theta))

s = sin(theta)

X = n.x

Y = n.y

Z = n.z

# Matrix 'M'

d11 = t*X**2 + c

d12 = t*X*Y - s*Z

d13 = t*X*Z + s*Y

d21 = t*X*Y + s*Z

d22 = t*Y**2 + c

d23 = t*Y*Z - s*X

d31 = t*X*Z - s*Y

d32 = t*Y*Z + s*X

d33 = t*Z**2 + c

#            |p.x|

# Matrix 'M'*|p.y|

#            |p.z|

q.x = d11*p.x + d12*p.y + d13*p.z

q.y = d21*p.x + d22*p.y + d23*p.z

q.z = d31*p.x + d32*p.y + d33*p.z

# Translate axis and rotated point back to original location

return q + self.p1

def lie_check(self, p):

"""

/// Given any point 'a' on a plane: N dot (a-p) = 0

"""

return round(self.dot_product(self.N, (p - self.p1)))

def version(self):

return "P3D.Plane3D Version 1.07"

################################################################

def test_Rotate3D():

from point import Point, PointLocate

from param import Prompt, Warning, dim_print, yes_or_no

from macrolib.angle import dtor, rtod

A = 45.0

pt1 = PointLocate("Pick center point of rotation")

pt2 = PointLocate("Pick point to rotate")

pt3 = PointLocate("Pick a counter-clockwise reference point to define the current plane")

print "Selected point: %s, %s, %s" % (dim_print(pt2.x), dim_print(pt2.y), dim_print(pt2.z))

while 1:

A = Prompt(A, "Enter rotation angle")

a = Plane3D(pt1, pt2, pt3, dtor(A))

pt = a.PointRotate3D(pt2, dtor(A))

Warning("\nSelected point: %s, %s, %s\nRotated point: %s, %s, %s" % \

(dim_print(pt2.x), dim_print(pt2.y), dim_print(pt2.z), dim_print(pt.x), dim_print(pt.y), dim_print(pt.z)))

print "Rotated point: %s, %s, %s" % (dim_print(pt.x), dim_print(pt.y), dim_print(pt.z))

if not yes_or_no("Rotate the point again?"):

break

## END test_Rotate3D() #########################################

def test_Plane3D():

from point import Point, PointLocate

from param import Warning

pt1 = PointLocate("Pick point 1")

pt2 = PointLocate("Pick point 2")

pt3 = PointLocate("Pick point 3")

if None not in [pt1, pt2, pt3]:

a = Plane3D(pt1, pt2, pt3)

if a.N_len > 0.0:

Warning('%s\n%s\n%s\n%s' % ('Normal unit vector to the plane defined by the picked points:',\

'x = ' + str(round(a.N_uv.x, 4)),\

'y = ' + str(round(a.N_uv.y, 4)),\

'z = ' + str(round(a.N_uv.z, 4))

)

)

print "Normal unit vector to the plane defined by the picked points ('N'):"

print 'x = ' + str(round(a.N_uv.x, 4))

print 'y = ' + str(round(a.N_uv.y, 4))

print 'z = ' + str(round(a.N_uv.z, 4)), '\n'

print "Plane constant 'k' = N dot product p:", a.k

print "Plane displacement from the origin (0,0,0) = N_uv dot product p:", a.k0, '\n'

print "a.D = %s" % (a.D)

pt4 = PointLocate("Pick a point to test lie_check")

if pt4:

if a.lie_check(pt4) == 0.0:

print "Selected point lies on current plane."

elif a.lie_check(pt4) < 0.0:

print "Selected point lies on OPPOSITE side of current plane as normal N."

else:

print "Selected point lies on SAME side of current plane as normal N."

print a.version()

print type(a)

print type(Plane3D)

if isinstance(a, Plane3D):

print 'a is an instance of class Plane3D'

else:

print 'This does not work'

print dir(a)

print repr(type(a)).split('.')[1][:-2]

print

print a

print

print repr(a)

print

print formatDict("Instance Dictionary", a.__dict__)

## END test_Plane3D() ##########################################

if __name__ == '__main__':

try:

#test_Rotate3D()

test_Plane3D()

finally:

del test_Rotate3D

del test_Plane3D

del Plane3D

""" Output from test_Plane3D()

Normal unit vector to the plane defined by the picked points ('N'):

x = -0.5507

y = 0.5365

z = -0.6394

Plane constant 'k' = N dot product p: -24436723.1888

Plane displacement from the origin (0,0,0) = N_uv dot product p: -7582.516842

a.D = 24436723.1888

Selected point lies on current plane.

P3D.Plane3D Version 1.07

<class '__main__.Plane3D'>

<type 'type'>

a is an instance of class Plane3D

['D', 'F', 'G', 'N', 'N_len', 'N_uv', 'PointRotate3D', 'Q', 'Ra', '__class__', '__delattr__', '__dict__', '__doc__', '__getattribute__', '__hash__', '__init__', '__module__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__str__', '__weakref__', 'cross_product', 'd', 'd0', 'dot_product', 'e', 'e0', 'k', 'k0', 'lie_check', 'p1', 'p2', 'p3', 'plane_2', 'plane_3', 'plane_def', 'pp', 'three_pt_circle', 'version']

Plane3D

Instance of Class Plane3D - Plane Definition Data

Normal Unit Vector: -0.5507, 0.5365, -0.6394

Displacement from Model Origin: -7582.5168

Plane Origin and Unit Vector Points

Origin point: 31'-1 3/4, 32'-4 1/2, 988'-6 1/8

Unit Vector d0: 0.8347, 0.3540, -0.4219

Unit Vector e0: 0.5350, -0.3611, -0.7638

Plane3D(Point(373.772672, 388.479871, 11862.096943), Point(457.377182, 423.934799, 11819.838516), Point(396.198661, 373.341282, 11830.081044), 0.875056)

Instance Dictionary

Key = D        Value = 24436723.1888

Key = F        Value = 415.574927, 406.207335, 11840.96773

Key = G        Value = 384.985667, 380.910576, 11846.088993

Key = N        Value = -1774.854353, 1728.986563, -2060.76613

Key = N_len    Value = 3222.772029

Key = N_uv     Value = -0.550723, 0.53649, -0.639439

Key = Q        Value = 0.875056407129

Key = Ra       Value = 100.162570884

Key = d        Value = 83.60451, 35.454928, -42.258426

Key = d0       Value = 0.834688, 0.353974, -0.421898

Key = e        Value = 22.425989, -15.138589, -32.015899

Key = e0       Value = 0.534996, -0.361148, -0.763774

Key = k        Value = -24436723.1888

Key = k0       Value = -7582.516842

Key = p1       Value = 373.772672, 388.479871, 11862.096943

Key = p2       Value = 457.377182, 423.934799, 11819.838516

Key = p3       Value = 396.198661, 373.341282, 11830.081044

Key = pp       Value = 87.6478994063

"""