CoordSystem
This class provides coordinate systems for the shapes.
When a CoordSystem is added to a shape, it's reference_CS is set. The one exception is when a Shape is created. When a shape is created, an Origin CoordSystem is created. The reference_CS for an Origin CoordSystem isn't set until the Shape is attached to a ComplexShape.
Contents
classdef CoordSystem < handle
% This defines the CoordSystem class. % URL : $URL: $ % Log : $Id: CoordSystem.html,v 1.1 2008/07/23 12:51:19 jberg Exp $ % Copyright (c) 2008 The MathWorks, Inc.
Class Properties
properties ( SetAccess = 'public', GetAccess = 'public' ) is_deletable = true; % (logical) is_fixed = false; % (logical) shape dependant CSs name = 'New CoordSystem'; % (char) orientation = [0 0 0]; % (3-element double) Euler directions [Rx,Ry,Rz] origin = [0 0 0]; % (3-element double) [x,y,z] parent_shape; % (Shape) triad; % (Triad) The reference for this CS reference_CS; % (CoordSystem) The reference for this CS end properties ( SetAccess = 'private', GetAccess = 'private', Constant = true ) PROPERTY_LIST = { 'is_deletable' 'is_fixed' 'name' 'orientation' 'origin' 'parent_shape' 'triad' }; end methods
Constructor
function obj = CoordSystem(varargin) prop_list = eval([mfilename '.PROPERTY_LIST']); n_props = length(prop_list); Log.msg(sprintf('\n%s::%s',mfilename,mfilename)); if(mod(nargin,2)~=0) error('Expecting inputs as property name-value pairs.') end error(nargchk(0, 2*n_props, nargin,'string')); % Property Name-Value pair matching for ii=1:2:nargin prop = varargin{ii}; val = varargin{ii+1}; idx = strmatch(lower(prop),lower(prop_list),'exact'); if isempty(idx) error('%s: This property is unknown ',... 'or cannot be set during the construction ',... 'of the object: %s ',prop,mfilename); else obj.(prop_list{idx}) = val; end end if isempty(obj.triad) obj.triad = Triad('name',obj.name); % default Triad end end
SET Methods
function set.is_deletable(obj, in) prop_name = 'is_deletable'; prop_type = 'logical'; [valid_flag in] = Validate.SCALAR(in,'type',prop_type); if valid_flag obj.(prop_name) = in; end end function set.is_fixed(obj, in) prop_name = 'is_fixed'; prop_type = 'logical'; [valid_flag in] = Validate.SCALAR(in,'type',prop_type); if valid_flag obj.(prop_name) = in; end end function set.name(obj, in) prop_name = 'name'; prop_type = 'char'; [valid_flag in] = Validate.ARRAY(in,'type',prop_type); if valid_flag obj.(prop_name) = in; end end function set.orientation(obj, in) prop_name = 'orientation'; prop_type = 'double'; [valid_flag in] = Validate.ARRAY(in,'type',prop_type,'minLen',3,'maxLen',3); if valid_flag obj.(prop_name) = in; obj.update; end end function set.origin(obj, in) prop_name = 'origin'; prop_type = 'double'; [valid_flag in] = Validate.ARRAY(in,'type',prop_type,'minLen',3,'maxLen',3); if valid_flag obj.(prop_name) = in; obj.update; end end function set.parent_shape(obj, in) prop_name = 'parent_shape'; prop_type = 'Shape'; [valid_flag in] = Validate.SCALAR(in,'type',prop_type); if valid_flag obj.(prop_name) = in; end end function set.reference_CS(obj, in) % This method is responsible for setting the reference CS for a CS. % It does not check for circular dependence because that is done % elsewhere where the case of circular dependence is better handled % (e.g., reseting the parent combobox to the previous selection). % Also, if the current reference CS for this CS is empty, then it is % assumed that this is being set for the first time, in which case % the position and orientation data isn't changed. Otherwise, the % position and orientation data is adjusted to be wrt the new % reference CS while maintaining the absolute position and % orientation of the CS. % % Part of assigning a reference CoordSystem is to attach the triad % of this object's Triad's BranchGroup to the TransformGroup of the % reference CoordSystem's Triad. This means that the Triad needs to % be defined for each CoordSystem, first. prop_name = 'reference_CS'; prop_type = 'CoordSystem'; [valid_flag in] = Validate.SCALAR(in,'type',prop_type); if valid_flag old_reference_CS = obj.reference_CS; if ~isempty(old_reference_CS) % Detach the CS's coordinate system from the TransformGroup % of the old reference CS old_TG = old_reference_CS.triad.TransformGroup; if ~isempty(old_TG) all_children = old_TG.getAllChildren; idx = 0; while all_children.hasMoreElements a_child = all_children.nextElement; if (a_child == obj.triad.BranchGroup) old_TG.removeChild(idx); break else idx = idx+1; end end end end % Attach the Triad's BranchGroup for this CoordSystem to % reference CoordSystem's Triad's TransformGroup. Clearly, this % assumes that both CoordSystems contain Triad objects. if ~isempty(in) && ~isempty(obj.triad) && ~isempty(in.triad) ... && ~isempty(obj.triad.BranchGroup) && ... ~isempty(in.triad.TransformGroup) %obj.triad.BranchGroup.compile(); % THIS DOING ANYTHING??? obj.triad.attach_BranchGroup(in.triad.TransformGroup) Log.msg(sprintf('\n%s::set.reference_CS\n Attaching %s\n ==> %s',... mfilename,... obj.triad.BranchGroup.toString.toCharArray,... in.triad.TransformGroup.toString.toCharArray)); end if ~isempty(obj.reference_CS) % update the CS object properties % Calculate origin and orientation for the new reference % such that the absolute position and orientation isn't % altered. [the_pos the_dir] = obj.convert_coordinates_wrt_new_refCS(in); obj.origin = the_pos; obj.orientation = the_dir; end obj.(prop_name) = in; end end function set.triad(obj, in) prop_name = 'triad'; prop_type = 'Triad'; [valid_flag in] = Validate.SCALAR(in,'type',prop_type); if valid_flag obj.(prop_name) = in; obj.update; end end
Public Helper Methods
function is_valid = check_if_valid_reference_CS(obj,ref_CS) is_valid = false; while(true) if(isempty(ref_CS)) is_valid = true; break; elseif(ref_CS == obj) break; else ref_CS = ref_CS.reference_CS; end end end function [origin euler_angles] = convert_local_coordinates_to_world(obj) % If the obj is already World.Origin or it's reference CS is % World.Origin, then dont do anything, just send back the obj's % origin and euler angles. if(strcmp(obj.name,'World.Origin') || ... strcmp(obj.reference_CS.name,'World.Origin')) origin = obj.origin; euler_angles = obj.orientation; return; end the_CS = obj; origin = (the_CS.origin)'; res_Q = [1 0 0 0]; % Identity Quaternion ref_CS = the_CS.reference_CS; % Keep looping until you reach world reference. while(~strcmp(the_CS.name,'World.Origin')) % To get the direction part Q = CoordSystem.EulerXYZ_to_Quaternion(the_CS.orientation); res_Q = CoordSystem.multiply_quaternions(Q,res_Q); % Note: Multiplication order is important % To get the coordinate values if(~strcmp(ref_CS.name,'World.Origin')) % Calculate the Transformation Matrices theta = (ref_CS.orientation)*pi/180; theta_x = theta(1); theta_y = theta(2); theta_z = theta(3); Rx = [1 0 0;... 0 cos(theta_x) -sin(theta_x);... 0 sin(theta_x) cos(theta_x)]; Ry = [cos(theta_y) 0 sin(theta_y);... 0 1 0;... -sin(theta_y) 0 cos(theta_y)]; Rz = [cos(theta_z) -sin(theta_z) 0;... sin(theta_z) cos(theta_z) 0;... 0 0 1]; % Following the Euler X-Y-Z rotation sequence ref_CS_coord = (ref_CS.origin)'; origin = Rz*Ry*Rx*origin + ref_CS_coord; end the_CS = ref_CS; ref_CS = the_CS.reference_CS; end origin = origin'; Q = CoordSystem.EulerXYZ_to_Quaternion(the_CS.orientation); res_Q = CoordSystem.multiply_quaternions(Q,res_Q); % Note: Multiplication order is important euler_angles = CoordSystem.Quaternion_to_EulerXYZ(res_Q); % Round to something reasonable origin((abs(origin) < 1e-10)) = 0; euler_angles((abs(euler_angles) < 1e-6)) = 0; end function [origin euler_angles] = convert_coordinates_wrt_new_refCS(obj,ref_CS) % This method calculates the position and orientation of one cs wrt % another. % Position of (CS2) wrt (CS1) is calculated as follows: % 1. Calculate the location of the CS2 wrt World.Origin frame % 2. Calculate the location of the CS1 wrt World.Origin frame % 3. Algebraically subtract the two results to get the location of % CS2 wrt CS1 in terms of World.Origin axes. % 4. Now post-multiply the location with the rotation sequence % [Rx][Ry][Rz] using the angle of CS1 wrt World.Origin in % order to place the location values in terms of the CS1 % frame. % The orientation of (CS2) wrt (CS1) is calculated as follows: % 5. Subtract the orientation of CS2 in the World.Origin frame % from that of CS1 in the World.Origin frame. % Location [ref_world_pos ref_world_theta] = ref_CS.convert_local_coordinates_to_world; [world_pos world_theta] = obj.convert_local_coordinates_to_world; origin = world_pos - ref_world_pos; theta = ref_world_theta*pi/180; theta_x = theta(1); theta_y = theta(2); theta_z = theta(3); Rx = [1 0 0;... 0 cos(theta_x) -sin(theta_x);... 0 sin(theta_x) cos(theta_x)]; Ry = [cos(theta_y) 0 sin(theta_y);... 0 1 0;... -sin(theta_y) 0 cos(theta_y)]; Rz = [cos(theta_z) -sin(theta_z) 0;... sin(theta_z) cos(theta_z) 0;... 0 0 1]; % Following the Euler X-Y-Z rotation sequence origin = origin(:)'*Rx*Ry*Rz; % Place in ref_world frame % Orientation euler_angles = world_theta - ref_world_theta; % Round to something reasonable origin((abs(origin) < 1e-10)) = 0; euler_angles((abs(euler_angles) < 1e-6)) = 0; end function change_origin_visible(obj, in) % This is used to place a small sphere at the origin to % symbolize World Origin. prop_type = 'logical'; [valid_flag in] = Validate.SCALAR(in,'type',prop_type); if valid_flag obj.triad.is_origin_visible = in; end end function update(obj) the_origin = obj.origin; % origin ([x,y,z]) the_orientation = obj.orientation*pi/180; % orientation ([Rx,Ry,Rz]) the_triad = obj.triad; if ~isempty(the_triad) && ~isempty(the_triad.TransformGroup) ... && ~isstruct(the_triad.TransformGroup) the_TG = the_triad.TransformGroup; tmpTrans = javax.media.j3d.Transform3D(); tmpTrans2 = javax.media.j3d.Transform3D(); tmpTrans.rotX(the_orientation(1)); tmpTrans2.rotY(the_orientation(2)); tmpTrans.mul(tmpTrans2); tmpTrans2.rotZ(the_orientation(3)); tmpTrans.mul(tmpTrans2); the_translation = javax.vecmath.Vector3d(); % Vector3d the_translation.set(the_origin(1), the_origin(2), the_origin(3)); tmpTrans.setTranslation(the_translation); the_TG.setTransform(tmpTrans); end end
end
methods ( Static = true)
LOADOBJ Method
function B = loadobj(A) % We use loadobj so that when a stored object is loaded, we can % perform necessary steps that are contained in the constructor. We % have to do this because the constructor is not executed when an % object is loaded. Log.msg(sprintf('\n%s::%s',mfilename,'loadobj (Entering)')); prop_list = eval([mfilename '.PROPERTY_LIST']); % Construct object using the stored properties storedFields = fieldnames(A); args = cell(0); for ii=1:length(storedFields) prop = storedFields{ii}; idx = strmatch(lower(prop),lower(prop_list),'exact'); if ~isempty(idx) args{end+1} = prop; %#ok<AGROW> args{end+1} = A.(storedFields{ii}); %#ok<AGROW> end end B = CoordSystem(args{:}); Log.msg(sprintf('%s::%s',mfilename,'loadobj (Exiting)')); end
Static Public HELPER Methods
function cs = get_by_name(cs_cellArray,cs_name) % This returns a single CoordSystem from a cell array of % CoordSystems. cs = []; if ~iscell(cs_cellArray) return else cs_idx = strmatch(cs_name,cellfun(@(x) x.name, cs_cellArray,'UniformOutput', false),'exact'); if ~isempty(cs_idx) cs = cs_cellArray{cs_idx(1)}; end end end function q = EulerXYZ_to_Quaternion(euler_angles) % The calculations are done using the Euler X-Y-Z sequence. % This function would not work for a different Euler rotation % sequence. % convert from deg to rad. Also you need a factor of 2 because % a rotation of theta corresponds to quaternion angle of % (theta/2) theta_x = euler_angles(1)*pi/180/2; theta_y = euler_angles(2)*pi/180/2; theta_z = euler_angles(3)*pi/180/2; q(1) = cos(theta_x)*cos(theta_y)*cos(theta_z) - ... sin(theta_x)*sin(theta_y)*sin(theta_z); q(2) = sin(theta_x)*cos(theta_y)*cos(theta_z) + ... cos(theta_x)*sin(theta_y)*sin(theta_z); q(3) = cos(theta_x)*sin(theta_y)*cos(theta_z) - ... sin(theta_x)*cos(theta_y)*sin(theta_z); q(4) = cos(theta_x)*cos(theta_y)*sin(theta_z) + ... sin(theta_x)*sin(theta_y)*cos(theta_z); end function euler_angles = Quaternion_to_EulerXYZ(q) % This function returns euler angles in degrees corresponding % to the Euler X-Y-Z sequence. This function would not work for % a different Euler rotation sequence. % NOTE: % The angles theta_x, theta_z will always be in the interval [-pi,pi] % The angle theta_y will always be in the interval [-pi/2,pi/2] q0 = q(1); q1 = q(2); q2 = q(3); q3 = q(4); % ATAN2(Y,X) gives results in the interval [-pi,pi]. To check the % quadrant, you need to use sign(Y), sign(X) to determine the % specific quadrant Y = -(2*q2*q3-2*q0*q1); X = (2*q0^2-1+2*q3^2); euler_angles(1) = atan2(Y,X); % ASIN(X) will give results in the interval [-pi/2 pi/2]. euler_angles(2) = asin(2*q1*q3+2*q0*q2); % Y = 2*q1*q3+2*q0*q2; % den = cos(euler_angles(1)); % if (den == 0) % euler_angles(2) = asin(Y); % else % X = (2*q0^2-1+2*q3^2)/den; % euler_angles(2) = atan2(Y,X); % end Y = -(2*q1*q2-2*q0*q3); X = (2*q0^2-1+2*q1^2); euler_angles(3) = atan2(Y,X); euler_angles = euler_angles*180/pi; end function q = multiply_quaternions(u,v) % The output is a 4 element row vector (quaternion) % The order of the two input quaternions is important; % Make the second quaternion a column vector just in case if it % is specified as a row vector v = reshape(v,4,1); q = [u(1) -u(2) -u(3) -u(4);... u(2) u(1) -u(4) u(3);... u(3) u(4) u(1) -u(2);... u(4) -u(3) u(2) u(1)]*v; q = reshape(q,1,4); end function DirCosineMat = EulerXYZ_to_DirCosineMatrix(euler_angles) % The calculations are done using the Euler X-Y-Z sequence. % This function would not work for a different Euler rotation % sequence. % convert from deg to rad. theta_x = euler_angles(1)*pi/180; theta_y = euler_angles(2)*pi/180; theta_z = euler_angles(3)*pi/180; Rx = [1 0 0; 0 cos(theta_x) sin(theta_x); 0 -sin(theta_x) cos(theta_x)]; Ry = [cos(theta_y) 0 -sin(theta_y); 0 1 0; sin(theta_y) 0 cos(theta_y)]; Rz = [cos(theta_z) sin(theta_z) 0; -sin(theta_z) cos(theta_z) 0; 0 0 1]; % Follow the Euler X-Y-Z sequence. DirCosineMat = Rz*Ry*Rx; end
end
end