MATLAB Geometry Classes (using homogeneous coordinates).

Line3.m

classdef Line3
    %LINE3 Creates a 3D line in homogeneous coordinates
    %   Factory:
    %   LINE3()     - A line at infinity
    %   LINE3(L)    - A copy of a line (L={Line3})
    %   LINE3(f,m)  - A line from an axis f and moment m (f=[3,1], m=[3,1])
    %   LINE3(P,e)  - A line through point P, along e (P={Point3}, e=[3,1])
    %   LINE3(W,r)  - A line through location r, perp to plane (W={Plane3}, r=[3,1])
    %   LINE3(P,W)  - A line though point, perpendicular to plane (P={Point3}, W={Plane3})
    %   LINE3(W,P)  - A line though point, perpendicular to plane (W={Plane3}, P={Point3})
    %   LINE3(P1,P2)- A line joining two points (P1={Point3}, P2={Point3})
    %   LINE3(W1,W2)- A line meeting two planes (W1={Plane3}, W2={Plane3})
    %
    %   Properties:
    %   Direction   - The direction vector of the line
    %   Location    - The vector location of the line origin
    %   ThroughPoint- The Point3 closest to the world origin
    %   Distance    - The minimum distance to the world origin
    %   Magnitude   - The magnitude of the free vector 
    %   Vector      - The free vector component   
    %   Moment      - The position depended vector component
    %   Normal      - Normal direction vector pointing away from the origin
    %   Binormal    - Normal direction vector pointing opposite of moment
    %
    %   Fields:
    %   f_      - The axis vector
    %   m_      - The moment vector
    
    properties (SetAccess = private)
        f_
        m_
    end
    
    properties (Dependent = true)
        Direction
        Position        
        ThroughPoint
        Distance
        Normal
        Binormal
        Vector
        Moment
        Magnitude
        Coordinates
        Equations
    end
    
    methods
        function obj = Line3(varargin)
            % Default values with line at infinity
            obj.f_ = o_;
            obj.m_ = o_;
            if(nargin==1)
                arg = varargin{1};
                if isa(arg, 'Line3')
                    obj.f_ = arg.f_;
                    obj.m_ = arg.m_;
                elseif max(size(arg))==3
                    obj.m_ = o_;
                    obj.f_ = arg(:);
                elseif max(size(arg))==6
                    obj.m_ = arg(1:3);
                    obj.f_ = arg(4:6);                
                end
            elseif nargin==2
                arg1 = varargin{1};
                arg2 = varargin{2};
                if isa(arg1,'Point3') && isa(arg2,'Point3')
                    obj.f_ = arg1.d*arg2.p_-arg2.d*arg1.p_;
                    obj.m_ = cr(arg1.p_) * arg2.p_;
                elseif isa(arg1,'Plane3') && isa(arg2,'Plane3')
                    obj.f_ = cr(arg1.w_)*arg2.w_;
                    obj.m_ = arg2.h*arg1.w_ - arg1.h*arg2.w_;
                elseif isa(arg1,'Plane3') && isa(arg2,'Point3')
                    obj.f_ = arg1.w_*arg2.d;
                    obj.m_ = cr(arg2.p_)*arg1.w_;
                elseif isa(arg1,'Point3') && isa(arg2,'Plane3')
                    obj.f_ = arg2.w_*arg1.d;
                    obj.m_ = cr(arg1.p_)*arg2.w_;                    
                elseif isa(arg1,'Plane3') && max(size(arg2))==3
                    obj.f_ = arg1.w_;
                    obj.m_ = cr(arg2)*arg1.w_;                    
                elseif isa(arg1,'Point3') && max(size(arg2))==3
                    obj.f_ = arg2*arg1.d;
                    obj.m_ = cr(arg1.p_)*arg2;
                elseif max(size(arg1))==3 && max(size(arg2))==3
                    obj.m_ = arg1(:);
                    obj.f_ = arg2(:);                                        
                end
            end
        end               
        
        function P = Meets(obj, W)
            P = Point3(obj, W);
        end
        
        function W = Joins(obj, P)
            W = Plane3(obj, P);
        end       
        
        function n = CommonNormalWithLine(obj, other)
            % Finds the common normal line
            if ~isa(other,'Line3')
                other = Line3(other);
            end
            n = Line3( cr(obj.Coordinates)*other.Coordinates );
        end
        
        function crd = get.Coordinates(obj)
            crd = [ obj.m_; obj.f_ ];
        end
        
        function vec = get.Direction(obj)
            vec = obj.f_/norm(obj.f_);
        end
        
        function P = get.ThroughPoint(obj)
            P = Point3(cr(obj.f_)*obj.m_, obj.f_.'*obj.f_);
        end
        
        function pos = get.Position(obj)
            pos = cr(obj.f_)*obj.m_ / (obj.f_.'*obj.f_);
        end
        function del = get.Distance(obj)
            del = norm( cr(obj.m_)*obj.f_ ) / (obj.f_.'*obj.f_);
        end
        function vec = get.Normal(obj)
            vec = -cr(obj.m_)*obj.f_ / norm( cr(obj.m_)*obj.f_ );
        end
        function vec = get.Binormal(obj)
            vec = -obj.m_ / norm(obj.m_);
        end
        
        function vec = get.Vector(obj)
            vec = obj.f_;
        end
        function vec = get.Moment(obj)
            vec = obj.m_;
        end
        
        function mag = get.Magnitude(obj)
            mag = norm(obj.f_);
        end
        
        function eq = get.Equations(obj)
            e = norm(obj.f_);
            t = sym('t');
            p = str2sym('[x;y;z]');
            eq = p == (cr(obj.f_)*obj.m_+t*e*obj.f_)/(e*e);
        end
        
        function l = Normalized(obj)
            w = norm(obj.f_);
            l = Line3(obj.f_/w, obj.m_/w);
        end
               
        function res = double(obj)
            res = [obj.f_;obj.m_];
        end
        function res = sym(obj)
            res = [obj.f_;obj.m_];
        end
               
        function Q = ProjectPoint(L, P)
            if isa(P,'Point3')
                Q = Point3( P.d*cr(L.Vector)*L.Moment + L.Vector*(L.Vector.'*P.p_), P.d*L.Vector.'*L.Vector);
            else
                error('Unsupported Operation')
            end
        end
        
        function d = DistanceTo(L, P)
            if isa(P,'Point3')
                f = L.f_;
                m = L.m_;
                p = P.Vector;
                q = P.Scalar;
                d = norm(q*m+cr(f)*p)/(q*norm(f));
            else
                error('Unsupported Operation')
            end
        end
        
        function L = RotateAbout(obj, axis, angle)
            if isa(axis,'Line3')
                rc = axis.Position;
                z = axis.Direction;
                R = Rot(z,angle);
                f = R*obj.f_;
                m = R*obj.m_ + cr(R*rc-rc)*f;
                L = Line3(f, m);
            else
                error('axis must be a Line3')
            end
        end
        
        function res = SkewAngleWith(L,H)
            c = L.Vector.'*H.Vector;
            s = norm(cr(L.Vector)*H.Vector);
            res = atan2(s,c);
        end
        
        function h = Render(obj,len)
            if nargin<2
                len = 10;
            end
            q = [ ...
                obj.Position-len/2*obj.Direction, ...
                obj.Position+len/2*obj.Direction ].';
            
            h = plot3(q(:,1),q(:,2),q(:,3));
            %arrow(obj.Position,obj.Position+obj.Direction, [], [], [], 2, 1)            
        end
        
    end
    
end

Plane3.m

classdef Plane3
    %PLANE3 Creates a 3D point in homogeneous coordinates
    %   Factory:
    %   PLANE3()    - A plane at infinity
    %   PLANE3(W)   - A copy of a plane (W={Plane3})
    %   PLANE3(P)   - A plane through point P, with normal away from orign (P={Point3})
    %   PLANE3(w,h) - A plane from coordinates (w=[3,1], h=[1])
    %   PLANE3(h,w) - A plane from coordinates (h=[1], w=[3,1])
    %   PLANE3(P,n) - A plane through point P, with normal n, (P={Point3}, n=[3,1])
    %   PLANE3(L,P) - A plane joining a line L and a point P
    %
    %   Properties:
    %   Position    - The location of the point in 3D
    %   Distance    - The distance of the point from the origin
    %   Normal      - The unit vector away from the origin
    %
    %   Fields:
    %   w_          - The vector coordinate
    %   h           - The scalar coordinate
    
    properties (SetAccess = private)
        w_
        h
    end
    
    properties (Dependent = true)
        Position
        Distance
        Normal
        Vector
        Scalar
        Magnitude
        Coordinates
        Equation
    end
    
    methods
        
        function obj = Plane3(varargin)
            %Default values with plane at infinity
            obj.w_ = o_;
            obj.h = -1;
            if nargin==1
                arg = varargin{1};
                if isa(arg,'Plane3')
                    obj.w_ = arg.w_;
                    obj.h = arg.h;
                elseif isa(arg,'Point3')
                    obj.w_ = arg.d * arg.p_;
                    obj.h = -arg.p_.'*arg.p_;
                elseif max(size(arg))==3
                    obj.w_ = arg(:);
                    obj.h = -arg.'*arg;
                elseif max(size(arg))==4
                    obj.w_ = arg(1:3);
                    obj.h = arg(4);
                end
            elseif nargin==2
                arg1 = varargin{1};
                arg2 = varargin{2};
                if isa(arg1,'Line3') && isa(arg2,'Point3')
                    obj.w_ = cr(arg1.f_)*arg2.p_ + arg1.m_*arg2.d;
                    obj.h  = -arg1.m_.'*arg2.p_;
                elseif isa(arg1,'Point3') && isa(arg2,'Line3') 
                    obj.w_ = cr(arg1.p_)*arg2.f_ - arg1.d*arg2.m_;
                    obj.h  = arg1.p_.'*arg2.m_;
                elseif isa(arg1,'Point3') && isshape(arg2, [3,1])
                    obj.w_ = arg2(:)*arg1.d;
                    obj.h = -arg1.p_.'*arg2;
                elseif isshape(arg1, [3,1]) && isshape(arg2, [1,1])
                    obj.w_ = arg1(:);
                    obj.h = arg2;
                elseif isshape(arg1, [1,1]) && isshape(arg2, [3,1])
                    obj.w_ = arg2(:);
                    obj.h = arg1;
                end
            end
        end
        
        function L = Meets(obj, W)
            L = Line3(obj,W);
        end
        function res = RelativeTo(obj, P)
            res = [];
            if isa(P,'Point3')
                res = Plane3(obj.w_*P.d, P.d*obj.h-obj.w_.'*P.p_);
            end
        end
        
        function crd = get.Coordinates(obj)
            crd = [ obj.w_; obj.h ];
        end        
        
        function pos = get.Position(obj)
            pos = -obj.h*obj.w_/(obj.w_.'*obj.w_);
        end
        
        function vec = get.Normal(obj)
            vec = obj.w_/norm(obj.w_);
        end
        
        function del = get.Distance(obj)
            del = -obj.h/norm(obj.w_);
        end
        
        function vec = get.Vector(obj)
            vec = obj.w_;
        end
        function num = get.Scalar(obj)
            num = obj.h;
        end
        
        function mag = get.Magnitude(obj)
            mag = norm(obj.w_);
        end
        
        function eq = get.Equation(obj)
            p = str2sym('[x;y;z;1]');
            eq = p.'*[ obj.w_; obj.h ] == 0;
        end
        
        function W = Normalized(obj)
            m = norm(obj.w_);
            W = Plane3(obj.w_/m, obj.h/m);
        end
        
        function res = double(obj)
            res = [obj.w_;obj.h];
        end
        function res = sym(obj)
            res = [obj.w_;obj.h];
        end
    end
    
end

Point3.m

classdef Point3
    %POINT3 Creates a 3D point in homogeneous coordinates
    %   Factory:
    %   POINT3()    - A point at the origin
    %   POINT3(P)   - A copy of a point (P={Point3})
    %   POINT3(r)   - A point at a location (r=[3,1])
    %   POINT3(p)   - A point from coordinates (p=[4,1])
    %   POINT3(W)   - A point on a plane closest to origin (W={Plane3})
    %   POINT3(p,d) - A point from coordinates (p=[3,1], d=[1])
    %   POINT3(d,p) - A point from coordinates (d=[1], p=[3,1])
	%   POINT3(P,L) - A point where plane P meets line L
    %
    %   Properties:
    %   Position    - The location of the point in 3D
    %   Distance    - The distance of the point from the origin
    %   Normal      - The unit vector away from the origin
    %
    %   Fields:
    %   d           - The scalar coordinate
    %   p_          - The vector coordinate
    
    properties (SetAccess = private)
        p_
        d
    end
    
    properties (Dependent = true)
        Position
        Distance
        Normal
        Vector
        Scalar
        Magnitude
        Coordinates
    end
    
    methods
        
        function obj = Point3(varargin)
            %Default values with point at origin
            obj.p_ = o_;
            obj.d = 1;
            if nargin==1
                arg = varargin{1};
                if isa(arg,'Point3')
                    obj.p_ = arg.p_;
                    obj.d = arg.d;
                elseif isa(arg,'Plane3')
                    obj.p_ = -arg.h*arg.w_;
                    obj.d = arg.w_.'*arg.w_;
                elseif max(size(arg))==3
                    obj.p_ = arg(:);
                    obj.d = 1;
                elseif max(size(arg))==4
                    obj.p_ = arg(1:3);
                    obj.d = arg(4);
                end
            elseif nargin==2
                arg1 = varargin{1};
                arg2 = varargin{2};
                if isa(arg1,'Plane3') && isa(arg2,'Line3')
                    obj.p_ = arg1.h*arg2.f_+cr(arg1.w_)*arg2.m_;
                    obj.d  = arg1.w_.'*arg2.f_;
                elseif max(size(arg1))==3 && max(size(arg2))==1
                    obj.p_ = arg1(:);
                    obj.d = arg2;
                elseif max(size(arg1))==1 && max(size(arg2))==3
                    obj.p_ = arg2(:);
                    obj.d = arg1;
                end
            end
        end
        
        function L = Joins(obj, P)
            L = Line3(obj,P);
        end
        
        function res = RelativeTo(obj, P)
            res = [];
            if isa(P,'Point3')
                res = Point3(obj.d*P.d, P.d*obj.p_+obj.d*P.p_);
            end
        end
        
        function crd = get.Coordinates(obj)
            crd = [ obj.p_; obj.d ];
        end        
        
        function pos = get.Position(obj)
            pos = obj.p_/obj.d;
        end
        
        function vec = get.Normal(obj)
            vec = obj.p_/norm(obj.p_);
        end
        
        function del = get.Distance(obj)
            del = norm(obj.p_)/obj.d;
        end
        
        function vec = get.Vector(obj)
            vec = obj.p_;
        end
        function num = get.Scalar(obj)
            num = obj.d;
        end
        
        function mag = get.Magnitude(obj)
            mag = obj.d;
        end
        
        function P = Normalized(obj)
            P = Point3(obj.p_/obj.d,1);
        end        
       
        function res = double(obj)
            res = [obj.p_;obj.d];
        end
        
        function res = sym(obj)
            res = sym([obj.p_;obj.d]);
        end
        
        function d = DistanceTo(obj, P)
            if isa(P,'Point3')
                d = norm(obj.p_*P.d-obj.d*P.p_)/(obj.d*P.d);
            else
                error('Unsupported Operation')
            end
        end
                
        function h = Render(obj)
            q = obj.Position;
            h = scatter3(q(1),q(2),q(3));
        end
                
    end
    
end

TestGeometry3.m

disp('Define points')
A = Point3([0;2;0]) %#ok<*NOPTS>
B = Point3([0;1;5])
C = Point3([2;2;2])
P = Point3([3;2;1])

disp('Plane through C')
W_1 = Plane3(C)

disp('Line through A,B')
L = Line3(A,B)
disp('Plane through L and P')
W_2 = Plane3(L,P)

disp('Line through W_1 and W_2')
N = Line3(W_1,W_2)
disp('Check closest point to origin.');
disp('Value expected from Pro/E model is');
r_expect = [2.1923;1.7308;2.0769]
disp('Value calculated is');
r = N.Position
r_err = max(abs(r-r_expect))/norm(r_expect);
disp(sprintf('Error is %0.2f%%', r_err*100)); %#ok<*DSPS>