Back to SDS/2 Parametric Scripts

 

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

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

## All rights reserved.

## 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)

            /// Radius = self.Ra

            /// 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

"""