Mercurial > forge
changeset 9140:5ab591396320 octave-forge
mechanics: Major structure change
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/deprecated/ocframe_deprecated/SolveFrame.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,181 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {[@var{Reactions}, @var{Displacements}, @var{MemF}] = } SolveFrame(@var{joints}, @var{members}, @var{nodeloads}, @var{dist}, @var{point}) +## +## +## Solves a 2D frame with the matrix displacement method for +## the following input parameters: +## +## joints = [x , y, constraints ; ...] +## +## constraints=[x , y, rotation] free=0, supported=1 +## +## members = [nodeN, nodeF, E, I, A; ...] +## +## nodeloads = [node, Fx, Fy, Mz; ...] +## +## loads on members: +## +## dist = [membernum,FxN,FyN,FxF,FyF,a,b,local ; ...] for distributed loads +## where FxN and FyN are the loads on distance a from the near node +## (same with far node and distance b) +## local=1 if loads are on local axis, 0 if global +## +## point = [membernum,Fx,Fy,a,local; ...] +## where Fx and Fy are the loads on distance a from the node near +## local=1 if loads are on local axis, 0 if global +## +## Output is formated as follows (rownumber corresponds to +## node or member number): +## +## Reactions = [Fx,Fy,Mz;...] where NaN if it was a non supported dof +## +## Displacements = [x,y,rotation;...] +## +## MemF = [FxN, FyN, MzN, FxF, FyF, MzF; ...] +## @end deftypefn + +function [Reactions,Displacements,MemF]=SolveFrame(joints,members,nodeloads,dist,point) + if (nargin < 5) + usage("[Reactions,Displacements,MemF]=SolveFrame(joints,members,nodeloads,dist,point) Use the help command for more information"); + end + % calc info: + % Elements Axis + % y| + % | + % |___________________________ x + % Near far + % joints: [x, y, constraints; ...] 1= fixed + % members [nodeN,nodeF,E,I,A; ...] + % nodeloads [node,Fx,Fy,Mz; ...] + + P=D=zeros(rows(joints)*3,1); + %add nodal loads to P matrix + for load=1:rows(nodeloads) + c=node2code(nodeloads(load,1)); + for i=0:2 + P(c(1+i))=P(c(1+i))+nodeloads(load,2+i); + end + end + free=[]; %contains unconstrainted codenumbers + supported=[]; %contains constrainted codenumbers + for node=1:rows(joints) + c=node2code(node); + for i=3:5 + if (joints(node,i)==0) + free=[free,c(i-2)]; + else + supported=[supported,c(i-2)]; + end + end + end + + %% global equation + %% { P_f } [ K_ff K_fs ] { Delta_f } { P_F_f } + %% { } = [ ] . { } + { } + %% { P_s } [ K_sf K_ss ] { Delta_s } { P_F_s } + %% Solutions: + %% Delta_f = K_ff^-1 (P_f - P_F_f) + %% P_s = K_sf * Delta_f + P_F_s + + %stiffness matrix + [ K_ff, K_sf ] = GlobalStiffnessMatrixRegrouped (joints, members,free,supported); %K_ss, K_fs are not used and if not calculated script is faster + + + %nodal forces and equivalent nodal ones + [P_F,MemFEF]=EquivalentJointLoads(joints,members,dist,point); + + + %reduced matrices + P_f=P(free,:); + P_s=P(supported,:); + + P_F_f=P_F(free,:); + P_F_s=P_F(supported,:); + + %solution: find unknown displacements + + try + %A X = B => solve with cholesky => X = R\(R'\B) + r = chol (K_ff); + D_f=r\(r'\(P_f-P_F_f)); + catch + error("System is unstable because K_ff is singular. Please check the support constraints!"); + end_try_catch + + %TODO: make use of iterative solver as an option. How???? + + %[D_f, flag] = pcg(K_ff,P_f-P_F_f,1e-6,1000); + %if (flag==1) + %max iteration + % printf('max iteration'); + % try + % %A X = B => solve with cholesky => X = R\(R'\B) + % r = chol (K_ff); + % D_f=r\(r'\(P_f-P_F_f)); + % catch + % error("System is unstable because K_ff is singular. Please check the support constraints!"); + % end_try_catch + %end + %if (flag==3) + %K_ff was found not positive definite + % error("System is unstable because K_ff is singular. Please check the support constraints!"); + %end + + + + + D(free,1)=D_f; + + %solution: find unknown (reaction) forces + P_s=K_sf*D_f+P_F_s; + P(supported,1)=P_s; + + + %solution: find unknown member forces + MemF=[]; + for member=1:rows(members) + N=members(member,1); + F=members(member,2); + xN=joints(N,1); + yN=joints(N,2); + xF=joints(F,1); + yF=joints(F,2); + T=TransformationMatrix(xN,yN,xF,yF); + k=MemberStiffnessMatrix(sqrt((xN-xF)**2+(yN-yF)**2),members(member,3),members(member,4),members(member,5)); + c=[node2code(N),node2code(F)]; + MemF=[MemF;(k*T*D(c'))']; + end + MemF=MemF+MemFEF;%+fixed end forces + + %format codenumbers to real output + %TODO: not efficient enough due to A=[A;newrow] + Displacements=[]; + Reactions=[]; + for i=0:rows(joints)-1 + Displacements=[Displacements;D(1+3*i:3+3*i)']; + Reactions=[Reactions;P(1+3*i:3+3*i)']; + end + for i=1:rows(Reactions) + for j=1:columns(Reactions) + if (joints(i,j+2)==0) + Reactions(i,j)=NaN; + end + end + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/deprecated/ocframe_deprecated/Test.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,50 @@ +## -*- texinfo -*- +## @deftypefn {Function File} {} ocframe_ex3() +## Example of a planar frame. +## +## @end deftypefn +function Test() + UNP120=[210e9,363.990e-8,17.000e-4]; + RHS=[210e9,28.900e-8,8.730e-4]; + joints=[0,0,1,1,0; + 1.25,0,0,0,0; + 1.575,0,0,0,0; + 2.5,0,0,0,0; + 3.425,0,0,0,0; + 3.75,0,0,0,0; + 5.,0,0,1,0; + 0,1.5,0,0,0; + 1.25,1.5,0,0,0; + 2.5,1.5,0,0,0; + 3.75,1.5,0,0,0; + 5,1.5,0,0,0]; + members=[1,2,UNP120; + 2,3,UNP120; + 3,4,UNP120; + 4,5,UNP120; + 5,6,UNP120; + 6,7,UNP120; + 8,9,UNP120; + 9,10,UNP120; + 10,11,UNP120; + 11,12,UNP120; + 1,8,RHS; + 2,9,RHS; + 4,10,RHS; + 6,11,RHS; + 7,12,RHS]; + + nodeloads=[3,0,-30e3,0; + 5,0,-30e3,0; + 12,30e3,0,0]; + tic + [P1,D1,MemF1]=SolveFrame(joints,members,nodeloads,[],[]); + toc + tic + [P2,D2,MemF2]=SolveFrameOpt(joints,members,nodeloads,[],[]); + toc + + P1-P2 + D1-D2 + MemF1-MemF2 +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/deprecated/ocframe_deprecated/test_FixedEndForcesDist.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,43 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function [F1,F2,F3,F4,F5,F6]=test_FixedEndForcesDist(l,FxN,FyN,FxF,FyF,a,b) + % distributed load in local coords. + % a: distance from Near + % b: distance from Far + % FxN, FyN forces on start of load in local coordinates + % FxF, FyF forces on end of load in local coordinates + + x=linspace(a,l-b,100); #generate 100 intervals for riemann integration + dx=x(2)-x(1); + S=[]; + for i=1:columns(x) + Fy = (FyF-FyN)/(l-b-a)*(x(i)-a)+FyN; + Fx = (FxF-FxN)/(l-b-a)*(x(i)-a)+FxN; + [ F1,F2,F3,F4,F5,F6 ] = test_FixedEndForcesPnt(l,x(i),Fx,Fy); + S=[S;[ F1,F2,F3,F4,F5,F6 ]]; + end + for i=1:columns(S) + S(1,i)/=2; + S(rows(S),i)/=2; + end + F1 = sum(S(:,1))*dx; + F2 = sum(S(:,2))*dx; + F3 = sum(S(:,3))*dx; + F4 = sum(S(:,4))*dx; + F5 = sum(S(:,5))*dx; + F6 = sum(S(:,6))*dx; +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/deprecated/ocframe_deprecated/test_FixedEndForcesPnt.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,36 @@ +## Copyright (C) 2011 Johan +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 2 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with Octave; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +## test_FixedEndForcesPnt + +## Author: Johan <johan@johan-Satellite-L30> +## Created: 2011-10-02 + +function [ F1,F2,F3,F4,F5,F6 ] = test_FixedEndForcesPnt (l,a,Fx,Fy) + joints=[0,0,1,1,1; + a,0,0,0,0; + l,0,1,1,1]; + nodeloads=[2,Fx,Fy,0]; + members=[1,2,1,1,1; + 2,3,1,1,1]; + R=SolveFrame(joints,members,nodeloads,[],[]); + F1 = R(1,1); + F2 = R(1,2); + F3 = R(1,3); + F4 = R(3,1); + F5 = R(3,2); + F6 = R(3,3); +endfunction
--- a/main/mechanics/inst/@rigidbody/display.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,82 +0,0 @@ -## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -## -## This program is free software; you can redistribute it and/or modify -## it under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or -## (at your option) any later version. -## -## This program is distributed in the hope that it will be useful, -## but WITHOUT ANY WARRANTY; without even the implied warranty of -## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -## GNU General Public License for more details. -## -## You should have received a copy of the GNU General Public License -## along with this program; if not, see <http://www.gnu.org/licenses/>. - -function display(obj) - - fields = fieldnames (obj); - - printf("\nRigid Body Object\n\n"); - for i = 1 : numel(fields) - name = fields{i}; - content = obj.(name); - - if isstruct (content) - - printf("\n"); - printf (" %s is a scalar strcuture with the fields:\n",name); - subf = fieldnames (content); - - for j = 1 : numel(subf) - - name = subf{j}; - scont = content.(name); - dim = size (scont); - typep = class (scont); - printFormatted(scont,typep,dim,name); - - end - - else - - dim = size (content); - typep = class (content); - printFormatted(content,typep,dim,name); - - end - - endfor - - printf("\n"); - -endfunction - -function printFormatted(cont,typep,dim,name) - - if !isempty(cont) - switch typep - case 'double' - - if isscalar(cont) - printf (" %s = %g\n", name, cont); - elseif max (size (cont)) < 10 - printf (" %s =\n", name); - str = disp(cont); - str = strsplit (str, "\n",true); - cellfun(@(x) printf (" %s\n", x), str); - printf("\n"); - else - printf (" %s = %dx%d %s\n", name, dim, [typep 'array']); - end - - otherwise - - printf (" %s = %dx%d %s\n", name, dim, typep); - - end - else - printf (" %s = [](0x0)\n", name); - end - -endfunction
--- a/main/mechanics/inst/@rigidbody/plot.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,76 +0,0 @@ -## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -## -## This program is free software; you can redistribute it and/or modify -## it under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or -## (at your option) any later version. -## -## This program is distributed in the hope that it will be useful, -## but WITHOUT ANY WARRANTY; without even the implied warranty of -## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -## GNU General Public License for more details. -## -## You should have received a copy of the GNU General Public License -## along with this program; if not, see <http://www.gnu.org/licenses/>. - -## -*- texinfo -*- -## @deftypefn {Function File} {@var{obj} =} plot () -## Plots the rigid body. -## -## @end deftypefn - -function h = plot(obj, varargin) - - ax = []; - if !isempty (varargin) - ax = varargin{1}; - end - - ## Center of mass - c = obj.CoM; - ## Shape - has_shape = !isinf (obj.InertiaMoment); - if has_shape - shapedata = obj.Shape.Data; - if iscell (shapedata) - shapedata = shape2polygon (shapedata); - end - - # Rotate - R = cos(obj.Angle)*eye(2) + sin(obj.Angle)*[0 1; -1 0]; - shapedata = shapedata * R; - - ## Baricenter - b = c + obj.Shape.Offset * R;; - - # Translate - shapedata = bsxfun(@plus, shapedata, b); - xs = shapedata(:,1); - ys = shapedata(:,2); - end - - ## Plot - if isempty (ax) - cla - if has_shape - h(3,1) = patch(xs,ys,'r','edgecolor','k'); - hold on - h(2,1) = plot(b(1),b(2),'sk'); - end - h(1,1) = plot(c(1),c(2),'ok','markerfacecolor','k'); - hold off - else - if has_shape - h(3,1) = patch(xs,ys,'r','edgecolor','k'); - hold on - h(2,1) = plot(ax,b(1),b(2),'sk'); - end - h(1,1) = plot(ax,c(1),c(2),'ok','markerfacecolor','k'); - hold off - end - - axis equal - ## Velocity - #TODO - -endfunction
--- a/main/mechanics/inst/@rigidbody/rigidbody.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,232 +0,0 @@ -## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -## -## This program is free software; you can redistribute it and/or modify -## it under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or -## (at your option) any later version. -## -## This program is distributed in the hope that it will be useful, -## but WITHOUT ANY WARRANTY; without even the implied warranty of -## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -## GNU General Public License for more details. -## -## You should have received a copy of the GNU General Public License -## along with this program; if not, see <http://www.gnu.org/licenses/>. - -## -*- texinfo -*- -## @deftypefn {Function File} {@var{obj} =} rigidbody () -## @deftypefnx {Function File} {@var{obj} =} rigidbody (@var{param},@var{value}) -## Create object @var{obj} of the rigid_body class. -## -## If no input arguments are provided the object is filled with default values. -## Valid values for @var{param} are: -## -## @strong{Mass}: Scalar. Sets the total mass of the body. -## -## @strong{CoM}: 1x2 matrix. Sets the position of the center of mass of the body. -## -## @strong{Angle}: Scalar. Sets the angle of the rigid body, respect to the -## positive x-axis. -## -## @strong{Shape}: A cell with the polynomial descriptions of the edges of the -## shape or a Nx2 matrix of vertices. If no shape is provided the body is assumed -## to be a point mass. The moment of inertia respect to an axis perpendicular to the -## plane passing through the center of mass of the body is calculated based on -## this shape. -## -## @strong{CoMOffset}: 1x2 matrix. Set the displacement of the baricenter of the -## shape respect to the center of mass. Use with care! Check demos. -## -## @end deftypefn - -function rigidbody = rigidbody(varargin) - - rigidbody = struct; - - ## DYNAMIC INFO ## - - ## CoM: Position of the center of mass of the body. - rigidbody.CoM = [0 0]; - - ## PricipalAxes: Coordinates of the principal axes of the shape. - rigidbody.PrincipalAxes = []; - - ## Angle: Respect to 1st axis of frame of reference. Angle of rotation of the - ## principal axes of body. - rigidbody.Angle = 0; - - ## Velocity: Respect to frame of reference. - rigidbody.Velocity = [0 0]; - - ## Angular speed: Rate of change of Angle - rigidbody.AngSpeed = 0; - - ## PARAMETRIC INFO ## - - ## Mass: Mass of the body. - rigidbody.Mass = 1; - - ## Moment of inertia - rigidbody.InertiaMoment = Inf; - - ## Shape: Shape of the body. - ## Data: Geometric description of the shape. - ## Principal Axes: Shape's principal axes. - rigidbody.Shape = struct('Data',[],'Offset',[0 0],'PrincipalAxes',[],... - 'AreaMoments',[]); - - if !isempty (varargin) - ## Parse param, value pairs - valid_params = {'Shape','Mass','CoM','CoMOffset','Angle'}; - varnames = cell(size(varargin)); - varnames(:) = ' '; - varnames(1:2:end) = varargin(1:2:end); - [pargiven idx] = ismember (valid_params, varnames); - - ## Shape given - if pargiven(1) - if iscell (varargin{idx(1)+1}) - shapedata = varargin{idx(1)+1}; - elseif !iscell(varargin{idx(1)+1}) - shapedata = polygon2shape(varargin{idx(1)+1}); - else - error('rigidBody:IvalidArgument','Unrecognized shape data.'); - end - - ## Fill Shape struct - ## Shape is always describd with axis with lower moment in positive - ## x-axis direction and centered in [0 0]. - baricenter = masscenter(shapedata); - - shapedata = shapetransform(shapedata, -baricenter(:)); - - [PA l] = principalaxes(shapedata); - ## Put 1st axis positive in x - if PA(1,1) < 0 ; - PA = -PA; - end - rigidbody.Shape.PrincipalAxes = PA; - rigidbody.Shape.AreaMoments = l; - - rigidbody.Shape.Data = shapetransform(shapedata, PA); - - rigidbody.InertiaMoment = inertiamoment (shapedata, rigidbody.Mass); - end - - ## Mass given - if pargiven(2) - - if isscalar (varargin{idx(2)+1}) - - rigidbody.Mass = varargin{idx(2)+1}; - # has shape? - if !isempty (rigidbody.Shape.Data) - rigidbody.InertiaMoment = rigidbody.Mass*rigidbody.InertiaMoment; - rigidbody.PrincipalAxes = rigidbody.Shape.PrincipalAxes; - else # is a point mass - rigidbody.InertiaMoment = Inf; - end - - elseif strcmp (class (varargin{idx(2)+1}), 'function_hanlde') - - ## TODO - error('rigidBody:Devel','Mass function, not yet implemented.'); - - else - error('rigidBody:IvalidArgument','Unrecognized mass data.'); - end - - end - - ## CoM given position of the CoM respect to frame of reference - if pargiven(3) - - com = varargin{idx(3)+1}; - dim = size(com); - if all(dim == [1 2]) - rigidbody.CoM = com; - else - error('rigidBody:IvalidArgument','Position of the body must ba 1x2 matrix.'); - end - end - - ## CoMOffset is minus the position of the shape respect to the cente rof mass - if pargiven(4) - - offset = varargin{idx(4)+1}; - dim = size(offset); - if all(dim == [1 2]) - rigidbody.Shape.Offset = - offset; - rigidbody.InertiaMoment += rigidbody.Mass*sumsq(offset); - else - error('rigidBody:IvalidArgument','Offset of CoM must ba 1x2 matrix.'); - end - end - - ## Angle in [0, 2*pi] in radians respect to the x-axis of the frame of reference. - if pargiven(5) - - if isscalar(varargin{idx(5)+1}) - rigidbody.Angle = mod(varargin{idx(5)+1}, 2*pi); - else - error('rigidBody:IvalidArgument','angle must be a scalar.'); - end - end - - end - - # TODO Check integrity - # CoM inside shape - - rigidbody = class (rigidbody, 'rigidbody'); - -endfunction - -%!demo -%! % Create an empty rigidbody -%! ebody = rigidbody(); -%! ebody -%! -%! % Create a mass point with mass ==2 at [0,0] -%! mp = rigidbody('Mass', 2); -%! mp -%! -%! % Create a mass point with mass == 1.5 at [1,1] -%! mp2 = rigidbody('Mass', 1.5,'CoM',[5,5]); -%! mp2 -%! -%! % Create a body with a shape -%! t = linspace(0,2*pi,64).'; -%! shape = [cos(t)-0.3*cos(3*t) sin(t)](1:end-1,:); -%! body = rigidbody('Mass', 1.5,'CoM',[-5,-5],'Shape',shape); -%! body -%! -%! % Create a body with a shape and an offset -%! shape = [cos(t)-0.3*cos(3*t) sin(t)](1:end-1,:); -%! body2 = rigidbody('Mass', 1,'CoM',[-5,-5],'Shape',shape,'CoMOffset',[0 0.1]); -%! body2 -%! -%! % --------------------------------------------------------------------------- -%! % Shows how to create rigid bodies. The case with Offset has to be treated -%! % carefully. It models a new rigid body with half the mass together with a -%! % point mass with the other half of the mass, such that the CoM moves away -%! % from the baricenter of the shape the given amount. The total mass is -%! % conserved. - -%!demo -%! % Create bodies -%! mp = rigidbody('Mass', 2); -%! mp.plot(); -%! -%! t = linspace(0,2*pi,64).'; -%! shape = [cos(t)-0.3*cos(3*t) sin(t)](1:end-1,:); -%! body = rigidbody('Mass', 1.5,'CoM',[-2,-2],'Shape',shape); -%! body.plot(gca) -%! -%! shape = [cos(t) sin(t)-0.3*cos(4*t)](1:end-1,:); -%! body2 = rigidbody('Mass', 1,'CoM',[2,2],'Shape',shape,'CoMOffset',[0 0.5]); -%! body2.plot(gca) -%! -%! % --------------------------------------------------------------------------- -%! % Shows how to plot rigid bodies -
--- a/main/mechanics/inst/@rigidbody/subsref.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,49 +0,0 @@ -## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -## -## This program is free software; you can redistribute it and/or modify -## it under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or -## (at your option) any later version. -## -## This program is distributed in the hope that it will be useful, -## but WITHOUT ANY WARRANTY; without even the implied warranty of -## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -## GNU General Public License for more details. -## -## You should have received a copy of the GNU General Public License -## along with this program; if not, see <http://www.gnu.org/licenses/>. - -function out = subsref (obj, idx) - - if ( !strcmp (class (obj), 'rigidbody') ) - error ("object must be of the rigidbody class but '%s' was used", class (obj) ); - elseif ( idx(1).type != '.' ) - error ("invalid index for class %s", class (obj) ); - endif - -# Error strings - method4field = "Class %s has no field %s. Use %s() for the method."; - typeNotImplemented = "%s no implemented for Class %s."; - - method = idx(1).subs; - - switch method - case 'plot' - - if numel (idx) == 1 % obj.plot doesn't exists - error (method4field, class (obj), method, method); - elseif strcmp (idx(2).type, '()') - if !isempty (idx(2).subs) - out = plot (obj, idx(2).subs{:}); - else - out = plot (obj); - end - else - error (typeNotImplemented,[method idx(2).type], class (obj)); - end - - otherwise - error ("invalid index for reference of class %s", class (obj) ); - endswitch - -endfunction
--- a/main/mechanics/inst/EAmatrix.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,308 +0,0 @@ -%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: youcan redistribute itand/or modify -%% it under the terms of the GNU General Public Licenseas publishedby -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it willbe useful, -%% but WITHOUTaNY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have receivedacopy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {@var{output} = } EAmatrix (@var{angles}, @var{convention}) -%% Generates the Eulerangle rotation matrix in the given convention. -%% -%% Using the equivalence between Euler angles and rotation composition, it is -%% possible to change to and from matrix convention. -%% Fixed (world)axes and column vectors, with intrinsic composition -%% (composition of rotations about body axes) ofactive rotationsand the -%% right-handed rule for the positive sign of the angles are assumed. This means -%% for example that a convention named (YXZ) is the result of performing firstan -%% intrinsic Y rotation, followedbyan Xanda Z rotations, in the moving axes. -%% -%% Subindexes refer to the order in which theangles are applied. Trigonometric -%% notation hasbeen simplified. For example,c1 meanscos(θ1)and s2 means -%% sin(θ2).as we assumed intrinsic and active compositions, θ1 is the external -%% angle of the static definition (angle between fixed axis x and line of nodes) -%% and θ3 the internal angle (from the line of nodes to rotated axis X). The -%% following table can be used both ways, to obtainan orientation matrix from -%% Euler angles and to obtain Euler angles from the matrix. The possible -%% combinations of rotations equivalent to Euler angles are shown here. -%% @end deftypefn - -function varargout = EAmatrix (ang,convention = [3 1 3], handle = false) - - if handle - - if !ischar (convention) - letters = 'XYZ'; - convention = letters(convention); - end - - varargout{1} = eval(["@" toupper(convention)]); - - if nargout > 1 - varargout{2} = eval(["@d" toupper(convention)]); - end - else - if ischar (convention) - convention =arrayfun (@str2num, ... - strrep( strrep( strrep ( toupper (convention), "X","1"),"Y","2"),"Z","3") ); - end - - I = eye(3)(convention,:); - R1 = rotv (I(1,:),-ang(1)); - %% Rotation of axes - M = R1 * rotv (I(2,:),-ang(2)) * rotv (I(3,:),-ang(3)); - - varargout{1} = M; - - if nargout > 1 - %% Conversion from angle derivatives to angular speed - R1e2 = I(2,:)*R1'; - varargout{2} = [ I(1,:)*M(:,1) R1e2*M(:,1) 0; ... - I(1,:)*M(:,2) R1e2*M(:,2) 0; ... - I(1,:)*M(:,3) R1e2*M(:,3) 1]; - end - end - -endfunction - -function M = ZXZ(ang) - c = cos(ang); - s = sin(ang); - M = [ c(1)*c(3) - s(1)*c(2)*s(3), -c(1)*s(3) - s(1)*c(2)*c(3), s(1)*s(2); ... - c(1)*c(2)*s(3) + s(1)*c(3), c(1)*c(2)*c(3) - s(1)*s(3), -c(1)*s(2); ... - s(2)*s(3), s(2)*c(3), c(2)]; -endfunction - -function dM = dZXZ(ang) - c = cos(ang); - s = sin(ang); - dM = [ s(2)*s(3),c(3),0; ... - s(2)*c(3),-s(3),0; ... - c(2),0,1]; - -endfunction - -function M = ZYZ(ang) - c = cos(ang); - s = sin(ang); - M = [ c(1)*c(2)*c(3)-s(1)*s(3), -c(1)*c(2)*s(3)-s(1)*c(3), c(1)*s(2);... - c(1)*s(3)+s(1)*c(2)*c(3), c(1)*c(3)-s(1)*c(2)*s(3), s(1)*s(2);... - -s(2)*c(3), s(2)*s(3), c(2)]; -endfunction - -function dM = dZYZ(ang) - c = cos(ang); - s = sin(ang); - dM = [ -s(2)*c(3),s(3),0; ... - s(2)*s(3),c(3),0; ... - c(2),0,1]; - -endfunction - -function M = XYX(ang) - c = cos(ang); - s = sin(ang); - M = [c(2),0,s(2); ... - c(1)*s(2)*s(3)+s(1)*s(2)*c(3),c(1)*c(3)-s(1)*s(3),-c(1)*c(2)*s(3)-s(1)*c(2)*c(3); ... - s(1)*s(2)*s(3)-c(1)*s(2)*c(3),c(1)*s(3)+s(1)*c(3),c(1)*c(2)*c(3)-s(1)*c(2)*s(3)]; -endfunction - -function dM = dXYX(ang) - c = cos(ang); - s = sin(ang); - dM = [ c(2),s(2)*s(3),0; ... - 0,c(3),0; ... - s(2),-c(2)*s(3),1]; - -endfunction - -function M = YXY(ang) - c = cos(ang); - s = sin(ang); - M = [c(1)*c(3)-s(1)*c(2)*s(3),s(1)*s(2),c(1)*s(3)+s(1)*c(2)*c(3); ... - s(2)*s(3),c(2),-s(2)*c(3); ... - -c(1)*c(2)*s(3)-s(1)*c(3),c(1)*s(2),c(1)*c(2)*c(3)-s(1)*s(3)]; -endfunction - -function dM = dYXY(ang) - c = cos(ang); - s = sin(ang); - dM = [ s(2)*s(3),c(3),0; ... - c(2),0,0; ... - -s(2)*c(3),s(3),1]; - -endfunction - -function M = XZY(ang) - c = cos(ang); - s = sin(ang); - M = [c(2)*c(3),-s(2),c(2)*s(3); ... - s(1)*s(3)+c(1)*s(2)*c(3),c(1)*c(2),c(1)*s(2)*s(3)-s(1)*c(3); ... - s(1)*s(2)*c(3)-c(1)*s(3),s(1)*c(2),s(1)*s(2)*s(3)+c(1)*c(3)]; -endfunction - -function dM = dXZY(ang) - c = cos(ang); - s = sin(ang); - dM = [ c(2)*c(3),-s(3),0; ... - -s(2),0,0; ... - c(2)*s(3),c(3),1]; - -endfunction - -function M = XYZ(ang) - c = cos(ang); - s = sin(ang); - M = [c(2)*c(3),-c(2)*s(3),s(2); ... - c(1)*s(3)+s(1)*s(2)*c(3),c(1)*c(3)-s(1)*s(2)*s(3),-s(1)*c(2); ... - s(1)*s(3)-c(1)*s(2)*c(3),c(1)*s(2)*s(3)+s(1)*c(3),c(1)*c(2)]; -endfunction - -function dM = dXYZ(ang) - c = cos(ang); - s = sin(ang); - dM = [ c(2)*c(3),s(3),0; ... - -c(2)*s(3),c(3),0; ... - s(2),0,1]; - -endfunction - -function M = YXZ(ang) - c = cos(ang); - s = sin(ang); - M = [s(1)*s(2)*s(3)+c(1)*c(3),s(1)*s(2)*c(3)-c(1)*s(3),s(1)*c(2); ... - c(2)*s(3),c(2)*c(3),-s(2); ... - c(1)*s(2)*s(3)-s(1)*c(3),s(1)*s(3)+c(1)*s(2)*c(3),c(1)*c(2)]; -endfunction - -function dM = dYXZ(ang) - c = cos(ang); - s = sin(ang); - dM = [ c(2)*s(3),c(3),0; ... - c(2)*c(3),-s(3),0; ... - -s(2),0,1]; - -endfunction - -function M = YZX(ang) - c = cos(ang); - s = sin(ang); - M = [s(1)*s(2)*s(3)+c(1)*c(2),s(1)*c(2)*s(3)-c(1)*s(2),s(1)*c(3); ... - s(2)*c(3),c(2)*c(3),-s(3); ... - c(1)*s(2)*s(3)-s(1)*c(2),c(1)*c(2)*s(3)+s(1)*s(2),c(1)*c(3)]; -endfunction - -function dM = dYZX(ang) - c = cos(ang); - s = sin(ang); - dM = [ s(2)*c(3),s(2)*s(3),0; ... - c(2)*c(3),c(2)*s(3),0; ... - -s(3),c(3),1]; - -endfunction - -function M = ZXY(ang) - c = cos(ang); - s = sin(ang); - M = [c(1)*c(3)-s(1)*s(2)*s(3),-s(1)*c(2),c(1)*s(3)+s(1)*s(2)*c(3); ... - c(1)*s(2)*s(3)+s(1)*c(3),c(1)*c(2),s(1)*s(3)-c(1)*s(2)*c(3); ... - -c(2)*s(3),s(2),c(2)*c(3)]; -endfunction - -function dM = dZXY(ang) - c = cos(ang); - s = sin(ang); - dM = [ -c(2)*s(3),c(3),0; ... - s(2),0,0; ... - c(2)*c(3),s(3),1]; - -endfunction - -function M = ZYX(ang) - c = cos(ang); - s = sin(ang); - M = [ c(1)*c(2)-s(1)*s(2)*s(3),-s(1)*c(3),s(1)*c(2)*s(3)+c(1)*s(2); ... - c(1)*s(2)*s(3)+s(1)*c(2),c(1)*c(3),s(1)*s(2)-c(1)*c(2)*s(3); ... - -s(2)*c(3),s(3),c(2)*c(3)]; -endfunction - -function dM = dZYX(ang) - c = cos(ang); - s = sin(ang); - dM = [ -s(2)*c(3),s(2)*s(3),0; ... - s(3),c(3),0; ... - c(2)*c(3),-c(2)*s(3),1]; - -endfunction - -function M = XZX(ang) - c = cos(ang); - s = sin(ang); - M = [ c(2),-s(2),0; ... - c(1)*s(2)*c(3)-s(1)*s(2)*s(3),c(1)*c(2)*c(3)-s(1)*c(2)*s(3),-c(1)*s(3)-s(1)*c(3); ... - c(1)*s(2)*s(3)+s(1)*s(2)*c(3),c(1)*c(2)*s(3)+s(1)*c(2)*c(3),c(1)*c(3)-s(1)*s(3)]; -endfunction - -function dM = dXZX(ang) - c = cos(ang); - s = sin(ang); - dM = [ c(2),s(2)*s(3),0; ... - -s(2),c(2)*s(3),0; ... - 0,c(3),1]; - -endfunction - -function M = YZY(ang) - c = cos(ang); - s = sin(ang); - M = [ c(1)*c(2)*c(3)-s(1)*s(3),-c(1)*s(2),c(1)*c(2)*s(3)+s(1)*c(3); ... - s(2)*c(3),c(2),s(2)*s(3); ... - -c(1)*s(3)-s(1)*c(2)*c(3),s(1)*s(2),c(1)*c(3)-s(1)*c(2)*s(3)]; -endfunction - -function dM = dYZY(ang) - c = cos(ang); - s = sin(ang); - dM = [ s(2)*c(3),-s(3),0; ... - c(2),0,0; ... - s(2)*s(3),c(3),1]; - -endfunction - -%!shared C, S, ANG -%! ANG = pi/2*eye(3); -%! C = cos(ANG); -%! S = sin(ANG); - -%!test -%! fhandle = EAmatrix (0,"ZXZ",true); -%! for i=1:3 -%! c = C(i,:); s = S(i,:); -%! ZXZ = [c(1)*c(3)-c(2)*s(1)*s(3) -c(1)*s(3)-c(3)*c(2)*s(1) s(2)*s(1); ... -%! c(2)*c(1)*s(3)+c(3)*s(1) c(1)*c(2)*c(3)-s(1)*s(3) -c(1)*s(2); ... -%! s(3)*s(2) c(3)*s(2) c(2)]; -%! -%! assert (EAmatrix (ANG(i,:)), ZXZ); -%! assert (fhandle(ANG(i,:)), ZXZ); -%! end - -%!test -%! [Mh dMh] = EAmatrix (0,"ZXZ",true); -%! [M dM] = EAmatrix (ANG(1,:),"ZXZ"); -%! c = C(1,:); s = S(1,:); -%! dZXZ = [ s(2)*s(3),c(3),0; ... -%! s(2)*c(3),-s(3),0; ... -%! c(2),0,1]; -%! -%! assert (dM, dZXZ); -%! assert (dMh(ANG(1,:)), dZXZ); - -
--- a/main/mechanics/inst/RBequations_rot.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,46 +0,0 @@ -%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: youcan redistribute itand/or modify -%% it under the terms of the GNU General Public Licenseas publishedby -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it willbe useful, -%% but WITHOUTaNY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have receivedacopy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {@var{dqdt} = } RBequations_rot (@var{t}, @var{q}, @var{opt}) -%% Rotational equations of motion of rigid body fixed in one point. -%% @end deftypefn - -function dqdt = RBequations_rot(t,q, opt) -% TODO -% 2. Actuation - - w = q(1:3,1).'; - s = q(4:7,1).'; - - I = opt.InertiaMoment; - m = opt.Mass; - Rcm = quatvrot(opt.CoM,s); - grav = opt.Gravity; - - - Tgrav = quatvrot((cross(Rcm,m*grav)),quatconj(s)); - - dqdt = zeros(7,1); - - %% Euler Equations - dqdt(1:3,1) = (cross( I .* w, w ) + Tgrav)./I; - - %% Quaternion equation - Omega = unvech ([0 ; q(1:3,1); 0; -q(3,1); q(2,1); 0; -q(1,1); 0],-1); - dqdt(4:7,1) = 0.5*Omega*q(4:7,1); - -endfunction -
--- a/main/mechanics/inst/RBexample.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,53 +0,0 @@ -%% Example of a free rigid body (no torques) - -%% Inital orientation -% The princicpal axis of the body in the [1 1 1] diagonal. We assume the body -% has the principal axis in the Y-direction - ang = acos(1/sqrt(3))*[-1 1 0]; - [R M] = EAmatrix(0,[3 1 3],true); - q0 = mat2quat(R(ang)); - -%% Inital angular velocities -% Body is rotating around its principal axis - w0 = [0 4*2*pi 0]; - - x0 = [w0 q0]; - -%% Set the system -% Circular cylinder with revolution axis in Y-direction - opt.Mass = 1; - r = 1; - h = 2; - opt.CoM = (3/4)*[0 h 0]; - opt.InertiaMoment = opt.Mass*[(3/5)*h^2 + (3/20)*r^2 (3/10)*r^2 (3/5)*h^2 + (3/20)*r^2]; - opt.Gravity = [0 0 -1]; - sys = @(t_,x_)RBequations_rot(t_,x_,opt); - -%% Set integration -tspan = [0 6]; -odeopt = odeset('RelTol',1e-3,'AbsTol',1e-3,... - 'InitialStep',1e-3,'MaxStep',1/10); - -%% Euler equations + Quaternions -tic -[t y] = ode45 (sys, tspan, x0, odeopt); -nT = length(t); -toc - -% Vector from vertex to end -r0 = [0 1 0]; - -%% quaternions -r = quatvrot (repmat(r0,nT,1), y(:,4:7) ); - -figure(1) -cla -drawAxis3D([0 0 0], eye(3), eye(3)); -line([0 r(1,1)],[0 r(1,2)],[0 r(1,3)],'color','k','linewidth',2); -hold on -plot3(r(:,1),r(:,2),r(:,3),'.k'); -hold off -axis tight; -axis equal; -axis square; -view(150,30)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/@rigidbody/display.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,82 @@ +## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program; if not, see <http://www.gnu.org/licenses/>. + +function display(obj) + + fields = fieldnames (obj); + + printf("\nRigid Body Object\n\n"); + for i = 1 : numel(fields) + name = fields{i}; + content = obj.(name); + + if isstruct (content) + + printf("\n"); + printf (" %s is a scalar strcuture with the fields:\n",name); + subf = fieldnames (content); + + for j = 1 : numel(subf) + + name = subf{j}; + scont = content.(name); + dim = size (scont); + typep = class (scont); + printFormatted(scont,typep,dim,name); + + end + + else + + dim = size (content); + typep = class (content); + printFormatted(content,typep,dim,name); + + end + + endfor + + printf("\n"); + +endfunction + +function printFormatted(cont,typep,dim,name) + + if !isempty(cont) + switch typep + case 'double' + + if isscalar(cont) + printf (" %s = %g\n", name, cont); + elseif max (size (cont)) < 10 + printf (" %s =\n", name); + str = disp(cont); + str = strsplit (str, "\n",true); + cellfun(@(x) printf (" %s\n", x), str); + printf("\n"); + else + printf (" %s = %dx%d %s\n", name, dim, [typep 'array']); + end + + otherwise + + printf (" %s = %dx%d %s\n", name, dim, typep); + + end + else + printf (" %s = [](0x0)\n", name); + end + +endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/@rigidbody/plot.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,76 @@ +## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program; if not, see <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {@var{obj} =} plot () +## Plots the rigid body. +## +## @end deftypefn + +function h = plot(obj, varargin) + + ax = []; + if !isempty (varargin) + ax = varargin{1}; + end + + ## Center of mass + c = obj.CoM; + ## Shape + has_shape = !isinf (obj.InertiaMoment); + if has_shape + shapedata = obj.Shape.Data; + if iscell (shapedata) + shapedata = shape2polygon (shapedata); + end + + # Rotate + R = cos(obj.Angle)*eye(2) + sin(obj.Angle)*[0 1; -1 0]; + shapedata = shapedata * R; + + ## Baricenter + b = c + obj.Shape.Offset * R;; + + # Translate + shapedata = bsxfun(@plus, shapedata, b); + xs = shapedata(:,1); + ys = shapedata(:,2); + end + + ## Plot + if isempty (ax) + cla + if has_shape + h(3,1) = patch(xs,ys,'r','edgecolor','k'); + hold on + h(2,1) = plot(b(1),b(2),'sk'); + end + h(1,1) = plot(c(1),c(2),'ok','markerfacecolor','k'); + hold off + else + if has_shape + h(3,1) = patch(xs,ys,'r','edgecolor','k'); + hold on + h(2,1) = plot(ax,b(1),b(2),'sk'); + end + h(1,1) = plot(ax,c(1),c(2),'ok','markerfacecolor','k'); + hold off + end + + axis equal + ## Velocity + #TODO + +endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/@rigidbody/rigidbody.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,232 @@ +## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program; if not, see <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {@var{obj} =} rigidbody () +## @deftypefnx {Function File} {@var{obj} =} rigidbody (@var{param},@var{value}) +## Create object @var{obj} of the rigid_body class. +## +## If no input arguments are provided the object is filled with default values. +## Valid values for @var{param} are: +## +## @strong{Mass}: Scalar. Sets the total mass of the body. +## +## @strong{CoM}: 1x2 matrix. Sets the position of the center of mass of the body. +## +## @strong{Angle}: Scalar. Sets the angle of the rigid body, respect to the +## positive x-axis. +## +## @strong{Shape}: A cell with the polynomial descriptions of the edges of the +## shape or a Nx2 matrix of vertices. If no shape is provided the body is assumed +## to be a point mass. The moment of inertia respect to an axis perpendicular to the +## plane passing through the center of mass of the body is calculated based on +## this shape. +## +## @strong{CoMOffset}: 1x2 matrix. Set the displacement of the baricenter of the +## shape respect to the center of mass. Use with care! Check demos. +## +## @end deftypefn + +function rigidbody = rigidbody(varargin) + + rigidbody = struct; + + ## DYNAMIC INFO ## + + ## CoM: Position of the center of mass of the body. + rigidbody.CoM = [0 0]; + + ## PricipalAxes: Coordinates of the principal axes of the shape. + rigidbody.PrincipalAxes = []; + + ## Angle: Respect to 1st axis of frame of reference. Angle of rotation of the + ## principal axes of body. + rigidbody.Angle = 0; + + ## Velocity: Respect to frame of reference. + rigidbody.Velocity = [0 0]; + + ## Angular speed: Rate of change of Angle + rigidbody.AngSpeed = 0; + + ## PARAMETRIC INFO ## + + ## Mass: Mass of the body. + rigidbody.Mass = 1; + + ## Moment of inertia + rigidbody.InertiaMoment = Inf; + + ## Shape: Shape of the body. + ## Data: Geometric description of the shape. + ## Principal Axes: Shape's principal axes. + rigidbody.Shape = struct('Data',[],'Offset',[0 0],'PrincipalAxes',[],... + 'AreaMoments',[]); + + if !isempty (varargin) + ## Parse param, value pairs + valid_params = {'Shape','Mass','CoM','CoMOffset','Angle'}; + varnames = cell(size(varargin)); + varnames(:) = ' '; + varnames(1:2:end) = varargin(1:2:end); + [pargiven idx] = ismember (valid_params, varnames); + + ## Shape given + if pargiven(1) + if iscell (varargin{idx(1)+1}) + shapedata = varargin{idx(1)+1}; + elseif !iscell(varargin{idx(1)+1}) + shapedata = polygon2shape(varargin{idx(1)+1}); + else + error('rigidBody:IvalidArgument','Unrecognized shape data.'); + end + + ## Fill Shape struct + ## Shape is always describd with axis with lower moment in positive + ## x-axis direction and centered in [0 0]. + baricenter = masscenter(shapedata); + + shapedata = shapetransform(shapedata, -baricenter(:)); + + [PA l] = principalaxes(shapedata); + ## Put 1st axis positive in x + if PA(1,1) < 0 ; + PA = -PA; + end + rigidbody.Shape.PrincipalAxes = PA; + rigidbody.Shape.AreaMoments = l; + + rigidbody.Shape.Data = shapetransform(shapedata, PA); + + rigidbody.InertiaMoment = inertiamoment (shapedata, rigidbody.Mass); + end + + ## Mass given + if pargiven(2) + + if isscalar (varargin{idx(2)+1}) + + rigidbody.Mass = varargin{idx(2)+1}; + # has shape? + if !isempty (rigidbody.Shape.Data) + rigidbody.InertiaMoment = rigidbody.Mass*rigidbody.InertiaMoment; + rigidbody.PrincipalAxes = rigidbody.Shape.PrincipalAxes; + else # is a point mass + rigidbody.InertiaMoment = Inf; + end + + elseif strcmp (class (varargin{idx(2)+1}), 'function_hanlde') + + ## TODO + error('rigidBody:Devel','Mass function, not yet implemented.'); + + else + error('rigidBody:IvalidArgument','Unrecognized mass data.'); + end + + end + + ## CoM given position of the CoM respect to frame of reference + if pargiven(3) + + com = varargin{idx(3)+1}; + dim = size(com); + if all(dim == [1 2]) + rigidbody.CoM = com; + else + error('rigidBody:IvalidArgument','Position of the body must ba 1x2 matrix.'); + end + end + + ## CoMOffset is minus the position of the shape respect to the cente rof mass + if pargiven(4) + + offset = varargin{idx(4)+1}; + dim = size(offset); + if all(dim == [1 2]) + rigidbody.Shape.Offset = - offset; + rigidbody.InertiaMoment += rigidbody.Mass*sumsq(offset); + else + error('rigidBody:IvalidArgument','Offset of CoM must ba 1x2 matrix.'); + end + end + + ## Angle in [0, 2*pi] in radians respect to the x-axis of the frame of reference. + if pargiven(5) + + if isscalar(varargin{idx(5)+1}) + rigidbody.Angle = mod(varargin{idx(5)+1}, 2*pi); + else + error('rigidBody:IvalidArgument','angle must be a scalar.'); + end + end + + end + + # TODO Check integrity + # CoM inside shape + + rigidbody = class (rigidbody, 'rigidbody'); + +endfunction + +%!demo +%! % Create an empty rigidbody +%! ebody = rigidbody(); +%! ebody +%! +%! % Create a mass point with mass ==2 at [0,0] +%! mp = rigidbody('Mass', 2); +%! mp +%! +%! % Create a mass point with mass == 1.5 at [1,1] +%! mp2 = rigidbody('Mass', 1.5,'CoM',[5,5]); +%! mp2 +%! +%! % Create a body with a shape +%! t = linspace(0,2*pi,64).'; +%! shape = [cos(t)-0.3*cos(3*t) sin(t)](1:end-1,:); +%! body = rigidbody('Mass', 1.5,'CoM',[-5,-5],'Shape',shape); +%! body +%! +%! % Create a body with a shape and an offset +%! shape = [cos(t)-0.3*cos(3*t) sin(t)](1:end-1,:); +%! body2 = rigidbody('Mass', 1,'CoM',[-5,-5],'Shape',shape,'CoMOffset',[0 0.1]); +%! body2 +%! +%! % --------------------------------------------------------------------------- +%! % Shows how to create rigid bodies. The case with Offset has to be treated +%! % carefully. It models a new rigid body with half the mass together with a +%! % point mass with the other half of the mass, such that the CoM moves away +%! % from the baricenter of the shape the given amount. The total mass is +%! % conserved. + +%!demo +%! % Create bodies +%! mp = rigidbody('Mass', 2); +%! mp.plot(); +%! +%! t = linspace(0,2*pi,64).'; +%! shape = [cos(t)-0.3*cos(3*t) sin(t)](1:end-1,:); +%! body = rigidbody('Mass', 1.5,'CoM',[-2,-2],'Shape',shape); +%! body.plot(gca) +%! +%! shape = [cos(t) sin(t)-0.3*cos(4*t)](1:end-1,:); +%! body2 = rigidbody('Mass', 1,'CoM',[2,2],'Shape',shape,'CoMOffset',[0 0.5]); +%! body2.plot(gca) +%! +%! % --------------------------------------------------------------------------- +%! % Shows how to plot rigid bodies +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/@rigidbody/subsref.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,49 @@ +## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program; if not, see <http://www.gnu.org/licenses/>. + +function out = subsref (obj, idx) + + if ( !strcmp (class (obj), 'rigidbody') ) + error ("object must be of the rigidbody class but '%s' was used", class (obj) ); + elseif ( idx(1).type != '.' ) + error ("invalid index for class %s", class (obj) ); + endif + +# Error strings + method4field = "Class %s has no field %s. Use %s() for the method."; + typeNotImplemented = "%s no implemented for Class %s."; + + method = idx(1).subs; + + switch method + case 'plot' + + if numel (idx) == 1 % obj.plot doesn't exists + error (method4field, class (obj), method, method); + elseif strcmp (idx(2).type, '()') + if !isempty (idx(2).subs) + out = plot (obj, idx(2).subs{:}); + else + out = plot (obj); + end + else + error (typeNotImplemented,[method idx(2).type], class (obj)); + end + + otherwise + error ("invalid index for reference of class %s", class (obj) ); + endswitch + +endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/EAmatrix.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,308 @@ +%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: youcan redistribute itand/or modify +%% it under the terms of the GNU General Public Licenseas publishedby +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it willbe useful, +%% but WITHOUTaNY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have receivedacopy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{output} = } EAmatrix (@var{angles}, @var{convention}) +%% Generates the Eulerangle rotation matrix in the given convention. +%% +%% Using the equivalence between Euler angles and rotation composition, it is +%% possible to change to and from matrix convention. +%% Fixed (world)axes and column vectors, with intrinsic composition +%% (composition of rotations about body axes) ofactive rotationsand the +%% right-handed rule for the positive sign of the angles are assumed. This means +%% for example that a convention named (YXZ) is the result of performing firstan +%% intrinsic Y rotation, followedbyan Xanda Z rotations, in the moving axes. +%% +%% Subindexes refer to the order in which theangles are applied. Trigonometric +%% notation hasbeen simplified. For example,c1 meanscos(θ1)and s2 means +%% sin(θ2).as we assumed intrinsic and active compositions, θ1 is the external +%% angle of the static definition (angle between fixed axis x and line of nodes) +%% and θ3 the internal angle (from the line of nodes to rotated axis X). The +%% following table can be used both ways, to obtainan orientation matrix from +%% Euler angles and to obtain Euler angles from the matrix. The possible +%% combinations of rotations equivalent to Euler angles are shown here. +%% @end deftypefn + +function varargout = EAmatrix (ang,convention = [3 1 3], handle = false) + + if handle + + if !ischar (convention) + letters = 'XYZ'; + convention = letters(convention); + end + + varargout{1} = eval(["@" toupper(convention)]); + + if nargout > 1 + varargout{2} = eval(["@d" toupper(convention)]); + end + else + if ischar (convention) + convention =arrayfun (@str2num, ... + strrep( strrep( strrep ( toupper (convention), "X","1"),"Y","2"),"Z","3") ); + end + + I = eye(3)(convention,:); + R1 = rotv (I(1,:),-ang(1)); + %% Rotation of axes + M = R1 * rotv (I(2,:),-ang(2)) * rotv (I(3,:),-ang(3)); + + varargout{1} = M; + + if nargout > 1 + %% Conversion from angle derivatives to angular speed + R1e2 = I(2,:)*R1'; + varargout{2} = [ I(1,:)*M(:,1) R1e2*M(:,1) 0; ... + I(1,:)*M(:,2) R1e2*M(:,2) 0; ... + I(1,:)*M(:,3) R1e2*M(:,3) 1]; + end + end + +endfunction + +function M = ZXZ(ang) + c = cos(ang); + s = sin(ang); + M = [ c(1)*c(3) - s(1)*c(2)*s(3), -c(1)*s(3) - s(1)*c(2)*c(3), s(1)*s(2); ... + c(1)*c(2)*s(3) + s(1)*c(3), c(1)*c(2)*c(3) - s(1)*s(3), -c(1)*s(2); ... + s(2)*s(3), s(2)*c(3), c(2)]; +endfunction + +function dM = dZXZ(ang) + c = cos(ang); + s = sin(ang); + dM = [ s(2)*s(3),c(3),0; ... + s(2)*c(3),-s(3),0; ... + c(2),0,1]; + +endfunction + +function M = ZYZ(ang) + c = cos(ang); + s = sin(ang); + M = [ c(1)*c(2)*c(3)-s(1)*s(3), -c(1)*c(2)*s(3)-s(1)*c(3), c(1)*s(2);... + c(1)*s(3)+s(1)*c(2)*c(3), c(1)*c(3)-s(1)*c(2)*s(3), s(1)*s(2);... + -s(2)*c(3), s(2)*s(3), c(2)]; +endfunction + +function dM = dZYZ(ang) + c = cos(ang); + s = sin(ang); + dM = [ -s(2)*c(3),s(3),0; ... + s(2)*s(3),c(3),0; ... + c(2),0,1]; + +endfunction + +function M = XYX(ang) + c = cos(ang); + s = sin(ang); + M = [c(2),0,s(2); ... + c(1)*s(2)*s(3)+s(1)*s(2)*c(3),c(1)*c(3)-s(1)*s(3),-c(1)*c(2)*s(3)-s(1)*c(2)*c(3); ... + s(1)*s(2)*s(3)-c(1)*s(2)*c(3),c(1)*s(3)+s(1)*c(3),c(1)*c(2)*c(3)-s(1)*c(2)*s(3)]; +endfunction + +function dM = dXYX(ang) + c = cos(ang); + s = sin(ang); + dM = [ c(2),s(2)*s(3),0; ... + 0,c(3),0; ... + s(2),-c(2)*s(3),1]; + +endfunction + +function M = YXY(ang) + c = cos(ang); + s = sin(ang); + M = [c(1)*c(3)-s(1)*c(2)*s(3),s(1)*s(2),c(1)*s(3)+s(1)*c(2)*c(3); ... + s(2)*s(3),c(2),-s(2)*c(3); ... + -c(1)*c(2)*s(3)-s(1)*c(3),c(1)*s(2),c(1)*c(2)*c(3)-s(1)*s(3)]; +endfunction + +function dM = dYXY(ang) + c = cos(ang); + s = sin(ang); + dM = [ s(2)*s(3),c(3),0; ... + c(2),0,0; ... + -s(2)*c(3),s(3),1]; + +endfunction + +function M = XZY(ang) + c = cos(ang); + s = sin(ang); + M = [c(2)*c(3),-s(2),c(2)*s(3); ... + s(1)*s(3)+c(1)*s(2)*c(3),c(1)*c(2),c(1)*s(2)*s(3)-s(1)*c(3); ... + s(1)*s(2)*c(3)-c(1)*s(3),s(1)*c(2),s(1)*s(2)*s(3)+c(1)*c(3)]; +endfunction + +function dM = dXZY(ang) + c = cos(ang); + s = sin(ang); + dM = [ c(2)*c(3),-s(3),0; ... + -s(2),0,0; ... + c(2)*s(3),c(3),1]; + +endfunction + +function M = XYZ(ang) + c = cos(ang); + s = sin(ang); + M = [c(2)*c(3),-c(2)*s(3),s(2); ... + c(1)*s(3)+s(1)*s(2)*c(3),c(1)*c(3)-s(1)*s(2)*s(3),-s(1)*c(2); ... + s(1)*s(3)-c(1)*s(2)*c(3),c(1)*s(2)*s(3)+s(1)*c(3),c(1)*c(2)]; +endfunction + +function dM = dXYZ(ang) + c = cos(ang); + s = sin(ang); + dM = [ c(2)*c(3),s(3),0; ... + -c(2)*s(3),c(3),0; ... + s(2),0,1]; + +endfunction + +function M = YXZ(ang) + c = cos(ang); + s = sin(ang); + M = [s(1)*s(2)*s(3)+c(1)*c(3),s(1)*s(2)*c(3)-c(1)*s(3),s(1)*c(2); ... + c(2)*s(3),c(2)*c(3),-s(2); ... + c(1)*s(2)*s(3)-s(1)*c(3),s(1)*s(3)+c(1)*s(2)*c(3),c(1)*c(2)]; +endfunction + +function dM = dYXZ(ang) + c = cos(ang); + s = sin(ang); + dM = [ c(2)*s(3),c(3),0; ... + c(2)*c(3),-s(3),0; ... + -s(2),0,1]; + +endfunction + +function M = YZX(ang) + c = cos(ang); + s = sin(ang); + M = [s(1)*s(2)*s(3)+c(1)*c(2),s(1)*c(2)*s(3)-c(1)*s(2),s(1)*c(3); ... + s(2)*c(3),c(2)*c(3),-s(3); ... + c(1)*s(2)*s(3)-s(1)*c(2),c(1)*c(2)*s(3)+s(1)*s(2),c(1)*c(3)]; +endfunction + +function dM = dYZX(ang) + c = cos(ang); + s = sin(ang); + dM = [ s(2)*c(3),s(2)*s(3),0; ... + c(2)*c(3),c(2)*s(3),0; ... + -s(3),c(3),1]; + +endfunction + +function M = ZXY(ang) + c = cos(ang); + s = sin(ang); + M = [c(1)*c(3)-s(1)*s(2)*s(3),-s(1)*c(2),c(1)*s(3)+s(1)*s(2)*c(3); ... + c(1)*s(2)*s(3)+s(1)*c(3),c(1)*c(2),s(1)*s(3)-c(1)*s(2)*c(3); ... + -c(2)*s(3),s(2),c(2)*c(3)]; +endfunction + +function dM = dZXY(ang) + c = cos(ang); + s = sin(ang); + dM = [ -c(2)*s(3),c(3),0; ... + s(2),0,0; ... + c(2)*c(3),s(3),1]; + +endfunction + +function M = ZYX(ang) + c = cos(ang); + s = sin(ang); + M = [ c(1)*c(2)-s(1)*s(2)*s(3),-s(1)*c(3),s(1)*c(2)*s(3)+c(1)*s(2); ... + c(1)*s(2)*s(3)+s(1)*c(2),c(1)*c(3),s(1)*s(2)-c(1)*c(2)*s(3); ... + -s(2)*c(3),s(3),c(2)*c(3)]; +endfunction + +function dM = dZYX(ang) + c = cos(ang); + s = sin(ang); + dM = [ -s(2)*c(3),s(2)*s(3),0; ... + s(3),c(3),0; ... + c(2)*c(3),-c(2)*s(3),1]; + +endfunction + +function M = XZX(ang) + c = cos(ang); + s = sin(ang); + M = [ c(2),-s(2),0; ... + c(1)*s(2)*c(3)-s(1)*s(2)*s(3),c(1)*c(2)*c(3)-s(1)*c(2)*s(3),-c(1)*s(3)-s(1)*c(3); ... + c(1)*s(2)*s(3)+s(1)*s(2)*c(3),c(1)*c(2)*s(3)+s(1)*c(2)*c(3),c(1)*c(3)-s(1)*s(3)]; +endfunction + +function dM = dXZX(ang) + c = cos(ang); + s = sin(ang); + dM = [ c(2),s(2)*s(3),0; ... + -s(2),c(2)*s(3),0; ... + 0,c(3),1]; + +endfunction + +function M = YZY(ang) + c = cos(ang); + s = sin(ang); + M = [ c(1)*c(2)*c(3)-s(1)*s(3),-c(1)*s(2),c(1)*c(2)*s(3)+s(1)*c(3); ... + s(2)*c(3),c(2),s(2)*s(3); ... + -c(1)*s(3)-s(1)*c(2)*c(3),s(1)*s(2),c(1)*c(3)-s(1)*c(2)*s(3)]; +endfunction + +function dM = dYZY(ang) + c = cos(ang); + s = sin(ang); + dM = [ s(2)*c(3),-s(3),0; ... + c(2),0,0; ... + s(2)*s(3),c(3),1]; + +endfunction + +%!shared C, S, ANG +%! ANG = pi/2*eye(3); +%! C = cos(ANG); +%! S = sin(ANG); + +%!test +%! fhandle = EAmatrix (0,"ZXZ",true); +%! for i=1:3 +%! c = C(i,:); s = S(i,:); +%! ZXZ = [c(1)*c(3)-c(2)*s(1)*s(3) -c(1)*s(3)-c(3)*c(2)*s(1) s(2)*s(1); ... +%! c(2)*c(1)*s(3)+c(3)*s(1) c(1)*c(2)*c(3)-s(1)*s(3) -c(1)*s(2); ... +%! s(3)*s(2) c(3)*s(2) c(2)]; +%! +%! assert (EAmatrix (ANG(i,:)), ZXZ); +%! assert (fhandle(ANG(i,:)), ZXZ); +%! end + +%!test +%! [Mh dMh] = EAmatrix (0,"ZXZ",true); +%! [M dM] = EAmatrix (ANG(1,:),"ZXZ"); +%! c = C(1,:); s = S(1,:); +%! dZXZ = [ s(2)*s(3),c(3),0; ... +%! s(2)*c(3),-s(3),0; ... +%! c(2),0,1]; +%! +%! assert (dM, dZXZ); +%! assert (dMh(ANG(1,:)), dZXZ); + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/RBequations_rot.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,46 @@ +%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: youcan redistribute itand/or modify +%% it under the terms of the GNU General Public Licenseas publishedby +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it willbe useful, +%% but WITHOUTaNY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have receivedacopy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{dqdt} = } RBequations_rot (@var{t}, @var{q}, @var{opt}) +%% Rotational equations of motion of rigid body fixed in one point. +%% @end deftypefn + +function dqdt = RBequations_rot(t,q, opt) +% TODO +% 2. Actuation + + w = q(1:3,1).'; + s = q(4:7,1).'; + + I = opt.InertiaMoment; + m = opt.Mass; + Rcm = quatvrot(opt.CoM,s); + grav = opt.Gravity; + + + Tgrav = quatvrot((cross(Rcm,m*grav)),quatconj(s)); + + dqdt = zeros(7,1); + + %% Euler Equations + dqdt(1:3,1) = (cross( I .* w, w ) + Tgrav)./I; + + %% Quaternion equation + Omega = unvech ([0 ; q(1:3,1); 0; -q(3,1); q(2,1); 0; -q(1,1); 0],-1); + dqdt(4:7,1) = 0.5*Omega*q(4:7,1); + +endfunction +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/RBexample.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,53 @@ +%% Example of a free rigid body (no torques) + +%% Inital orientation +% The princicpal axis of the body in the [1 1 1] diagonal. We assume the body +% has the principal axis in the Y-direction + ang = acos(1/sqrt(3))*[-1 1 0]; + [R M] = EAmatrix(0,[3 1 3],true); + q0 = mat2quat(R(ang)); + +%% Inital angular velocities +% Body is rotating around its principal axis + w0 = [0 4*2*pi 0]; + + x0 = [w0 q0]; + +%% Set the system +% Circular cylinder with revolution axis in Y-direction + opt.Mass = 1; + r = 1; + h = 2; + opt.CoM = (3/4)*[0 h 0]; + opt.InertiaMoment = opt.Mass*[(3/5)*h^2 + (3/20)*r^2 (3/10)*r^2 (3/5)*h^2 + (3/20)*r^2]; + opt.Gravity = [0 0 -1]; + sys = @(t_,x_)RBequations_rot(t_,x_,opt); + +%% Set integration +tspan = [0 6]; +odeopt = odeset('RelTol',1e-3,'AbsTol',1e-3,... + 'InitialStep',1e-3,'MaxStep',1/10); + +%% Euler equations + Quaternions +tic +[t y] = ode45 (sys, tspan, x0, odeopt); +nT = length(t); +toc + +% Vector from vertex to end +r0 = [0 1 0]; + +%% quaternions +r = quatvrot (repmat(r0,nT,1), y(:,4:7) ); + +figure(1) +cla +drawAxis3D([0 0 0], eye(3), eye(3)); +line([0 r(1,1)],[0 r(1,2)],[0 r(1,3)],'color','k','linewidth',2); +hold on +plot3(r(:,1),r(:,2),r(:,3),'.k'); +hold off +axis tight; +axis equal; +axis square; +view(150,30)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/drawAxis3D.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,21 @@ +%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: youcan redistribute itand/or modify +%% it under the terms of the GNU General Public Licenseas publishedby +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it willbe useful, +%% but WITHOUTaNY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have receivedacopy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +function drawAxis3D (p,E,c) + for i=1:3 + l =[p; p+E(i,:)]; + line(l(:,1),l(:,2),l(:,3),'color',c(i,:),'linewidth',2); + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/forcematrix.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,204 @@ +%% Copyright (c) 2010 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {[@var{F}, @var{D}] =} forcematrix (@var{pos}, @var{A},@var{funcs}) +%% Evaluates a force function depending on the separation of multiple interacting points. +%% +%% @strong{INPUT} +%% +%% @itemize +%% @item +%% @var{pos} is a Nxdim array of positions. N is the number of points and dim the +%% dimension. Each row represent a different point. +%% +%% @item +%% @var{A} is the a connectivity (adjacency) matrix expressed as a vector. That is, if +%% @var{M} is the complete NxN symmetric connectivity matrix, then +%% @code{@var{A}= vech(@var{M})}. The diagonal of @var{M} is not used. The elements +%% of A are indexes to the corresponding interaction force in @var{funcs}. For +%% example, there are 3 points and points 2 and 3 interact with a force +%% described in the 5th element of @var{funcs}, then @code{ M(2, 3) = 5}. +%% +%% @item +%% @var{funcs} is a Nfx1 cell. Elements that are matrices are evaluated with +%% @code{polyval}. elements that are function handles are evaluated directly. +%% Functions handles must be of the form @code{@@(@var{x})f(@var{x})}, where x is +%% the distance between two interacting points. +%% +%% @end itemize +%% +%% @strong{OUTPUT} +%% +%% @itemize +%% @item +%% @var{F} is a Nxdim array of forces acting on each point. +%% +%% @item +%% @var{D} is an array in the same format as @var{A} containing the euclidean +%% distances between the points. +%% @end itemize +%% +%% @html +%% @include doc/matrixforce.svg +%% @end html +%% @seealso{pointmassmesh, polyval, vech, sub2ind} +%% @end deftypefn + +function [F D] = forcematrix (pos, A, funcs) + + %% Argument parsing + [N dim] = size(pos); + nA = numel(A); + + % Relative position matrix + dr = zeros (nA,dim); + for ic = 1:dim + dr(:,ic) = vech (bsxfun (@minus, pos(:,ic)', pos(:,ic))); + end + % Distance vector + D = sqrt (sum (dr.^2, 2)); + % Versor + drhat = dr ./ repmat(D, 1,dim); + + %% Extend funcs to handle 0 elements. This or eliminate all diagonals. + funcs{end+1} = 0; + nf = numel(funcs); + A(A==0) = nf; + + %% pair to pair forces + f = cellfun (@handleOpoly, {funcs{A}}.', mat2cell (D, ones(nA,1), 1)); + f = repmat(f,1,dim) .* drhat; + + %% net force on p1 + F = zeros (N,dim); + + %% FIXME avoid looping + aux = ones(1,N-1); + p2 = sub2ind_tril (N, 2:N, aux); + F(1,:) = sum (f(p2,:)); + + p2 = sub2ind_tril (N, 1:N-1, N*aux ); + F(N,:) = -sum (f(p2,:)); + + for p1 = 2:N-1 + idx = sub2ind_tril (N, 1:N, repmat (p1,1,N) ); + sthalf = idx(1:(p1-1)); + ndhalf = idx((p1+1):N); + F(p1,:) = sum (f(ndhalf,:)) - sum (f(sthalf,:)); + end + +endfunction + +function f = handleOpoly (y,x) + + if isnumeric(y) + f = polyval(y,x); + else + f = y(x); + end + +end + +%% 1D +%!test +%! pos = [0;1]; A=vech ([0 1; 1 0]); funcs={[-1 1]}; +%! [F D] = forcematrix (pos, A, funcs); +%! assert (F, [0; 0], 1e-8); +%! assert (D, [0; 1; 0]); + +%!test +%! pos = [0;1;2]; A=vech ([0 1 0; 1 0 1; 0 1 0]); funcs={[-1 1]}; +%! [F D] = forcematrix (pos, A, funcs); +%! assert (F, [0; 0; 0], 1e-8); +%! assert (D, [0; 1; 2; 0; 1; 0]); + +%!test +%! pos = [0;1;2]; A=vech ([0 1 0; 1 0 1; 1 0 0]); funcs={[-1 1]}; +%! [F D] = forcematrix (pos, A, funcs); +%! assert (F, [1; 0; -1], 1e-8); +%! assert (D, [0; 1; 2; 0; 1; 0]); + +%!test +%! pos = [0;1;2]; A=vech ([0 1 0; 1 0 1; 1 1 0]); funcs={[-1 1]}; +%! [F D] = forcematrix (pos, A, funcs); +%! assert (F, [1; 0; -1], 1e-8); +%! assert (D, [0; 1; 2; 0; 1; 0]); + +%% 2D +%!test % Equilateral triangle +%! pos = [0 0;1 0;cos(pi/3) sin(pi/3)]; A=[0;1;1; 0;1; 0]; funcs={[-1 1]}; +%! [F D] = forcematrix (pos, A, funcs); +%! assert (F, [0 0; 0 0; 0 0], 1e-8); +%! assert (D, [0; 1; 1; 0; 1; 0], 1e-8); + +%!test % Square +%! pos = [0 0;1 0;1 1;0 1]; A=[0;1;0;1; 0;1;0; 0;1; 0]; funcs={[-1 1]}; +%! [F D] = forcematrix (pos, A, funcs); +%! assert (F, [0 0; 0 0; 0 0; 0 0], 1e-8); +%! assert (D, [0;1;sqrt(2);1; 0;1;sqrt(2); 0;1; 0], 1e-8); + + +% 3D +%!test % Cube +%! pos = [0 0 0; 1 0 0; 1 1 0; 0 1 0; ... +%! 0 0 1; 1 0 1; 1 1 1; 0 1 1;]; +%! A= [0;1;0;1;1;0;0;0; ... +%! 0;1;0;0;1;0;0; ... +%! 0;1;0;0;1;0; ... +%! 0;0;0;0;1; ... +%! 0;1;0;1; ... +%! 0;1;0; ... +%! 0;1; ... +%! 0; ]; +%! funcs={[-1 1]}; +%! [F D] = forcematrix (pos, A, funcs); +%! assert (F, zeros(8,3), 1e-8); +%! D_sb = [0;sqrt([1;2;1;1;2;3;2]); ... +%! 0;sqrt([1;2;2;1;2;3]); ... +%! 0;sqrt([1;3;2;1;2]); ... +%! 0;sqrt([2;3;2;1]); ... +%! 0;sqrt([1;2;1]); ... +%! 0;sqrt([1;2]); ... +%! 0;sqrt(1); ... +%! 0]; +%! assert (D, D_sb, 1e-8); + +%!test % Cube 2 funcs +%! pos = [0 0 0; 1 0 0; 1 1 0; 0 1 0; ... +%! 0 0 1; 1 0 1; 1 1 1; 0 1 1;]; +%! A= [0;1;0;1;2;0;0;0; ... +%! 0;1;0;0;2;0;0; ... +%! 0;1;0;0;2;0; ... +%! 0;0;0;0;2; ... +%! 0;1;0;1; ... +%! 0;1;0; ... +%! 0;1; ... +%! 0; ]; +%! funcs={[-1 1], @(x)-x}; +%! [F D] = forcematrix (pos, A, funcs); +%! F_sb = [zeros(4,2) ones(4,1); ... +%! zeros(4,2) -ones(4,1)]; +%! assert (F, F_sb, 1e-8); +%! D_sb = [0;sqrt([1;2;1;1;2;3;2]); ... +%! 0;sqrt([1;2;2;1;2;3]); ... +%! 0;sqrt([1;3;2;1;2]); ... +%! 0;sqrt([2;3;2;1]); ... +%! 0;sqrt([1;2;1]); ... +%! 0;sqrt([1;2]); ... +%! 0;sqrt(1); ... +%! 0]; +%! assert (D, D_sb, 1e-8); +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/inertiamoment.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,75 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software; you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation; either version 3 of the License, or +%% (at your option) any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program; if not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} { @var{Iz} =} inertiamoment (@var{pp}, @var{m}) +%% Moment of intertia of a plane shape. +%% +%% Calculates the moment of inertia respect to an axis perpendicular to the +%% plane passing through the center of mass of the body. +%% +%% The shape is defined with piecewise smooth polynomials. @var{pp} is a +%% cell where each elements is a 2-by-(poly_degree+1) matrix containing px(i,:) = +%% pp{i}(1,:) and py(i,:) = pp{i}(2,:). +%% +%% @seealso{masscenter, principalaxis} +%% @end deftypefn + +function Iz = inertiamoment (pp, m) + + Iz = sum(cellfun (@Iint, pp)); + A = shapearea(pp); + Iz = m*Iz / A; + +endfunction + +function dI = Iint (x) + + px = x(1,:); + py = x(2,:); + + paux = conv (px, px)/3 + conv(py, py); + paux2 = conv (px, polyderiv (py)) ; + P = polyint (conv (paux, paux2)); + + %% Faster than quad + dI = diff(polyval(P,[0 1])); + +endfunction + +%!demo % non-convex bezier shape +%! weirdhearth ={[-17.6816 -34.3989 7.8580 3.7971; ... +%! 15.4585 -28.3820 -18.7645 9.8519]; ... +%! [-27.7359 18.1039 -34.5718 3.7878; ... +%! -40.7440 49.7999 -25.5011 2.2304]}; +%! Iz = inertiamoment (weirdhearth,1) + +%!test +%! square = {[1 -0.5; 0 -0.5]; [0 0.5; 1 -0.5]; [-1 0.5; 0 0.5]; [0 -0.5; -1 0.5]}; +%! Iz = inertiamoment (square,1); +%! assert (1/6, Iz, sqrt(eps)); + +%!test +%! circle = {[1.715729 -6.715729 0 5; ... +%! -1.715729 -1.568542 8.284271 0]; ... +%! [1.715729 1.568542 -8.284271 0; ... +%! 1.715729 -6.715729 0 5]; ... +%! [-1.715729 6.715729 0 -5; ... +%! 1.715729 1.568542 -8.284271 0]; ... +%! [-1.715729 -1.568542 8.284271 0; ... +%! -1.715729 6.715729 0 -5]}; +%! Iz = inertiamoment (circle,1); +%! assert (Iz, 0.5*5^2, 5e-3); +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/masscenter.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,71 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software; you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation; either version 3 of the License, or +%% (at your option) any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program; if not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} { @var{cm} =} masscenter (@var{pp}) +%% Center of mass of a plane shape. +%% +%% The shape is defined with piecewise smooth polynomials. @var{pp} is a +%% cell where each elements is a 2-by-(poly_degree+1) matrix containing px(i,:) = +%% pp{i}(1,:) and py(i,:) = pp{i}(2,:). +%% +%% @seealso{inertiamoment} +%% @end deftypefn + +function cm = masscenter (shape) + + cm = sum( cell2mat ( cellfun (@CMint, shape, 'UniformOutput', false))); + A = shapearea(shape); + cm = cm / A; + +endfunction + +function dcm = CMint (x) + + px = x(1,:); + py = x(2,:); + Px = polyint (conv(conv (px , px)/2 , polyderiv (py))); + Py = polyint (conv(-conv (px , px)/2 , polyderiv (px))); + + dcm = zeros (1,2); + dcm(1) = diff(polyval(Px,[0 1])); + dcm(2) = diff(polyval(Py,[0 1])); + +endfunction + +%!demo % non-convex bezier shape +%! weirdhearth ={[-17.6816 -34.3989 7.8580 3.7971; ... +%! 15.4585 -28.3820 -18.7645 9.8519]; ... +%! [-27.7359 18.1039 -34.5718 3.7878; ... +%! -40.7440 49.7999 -25.5011 2.2304]}; +%! CoM = masscenter (weirdhearth) + +%!test +%! square = {[1 -0.5; 0 -0.5]; [0 0.5; 1 -0.5]; [-1 0.5; 0 0.5]; [0 -0.5; -1 0.5]}; +%! CoM = masscenter (square); +%! assert (CoM, [0 0], sqrt(eps)); + +%!test +%! circle = {[1.715729 -6.715729 0 5; ... +%! -1.715729 -1.568542 8.284271 0]; ... +%! [1.715729 1.568542 -8.284271 0; ... +%! 1.715729 -6.715729 0 5]; ... +%! [-1.715729 6.715729 0 -5; ... +%! 1.715729 1.568542 -8.284271 0]; ... +%! [-1.715729 -1.568542 8.284271 0; ... +%! -1.715729 6.715729 0 -5]}; +%! CoM = masscenter (circle); +%! assert (CoM , [0 0], 5e-3); +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/mat2quat.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,17 @@ +function q = mat2quat(R) + + if all(R == eye(3)) + q = [1 0 0 0]; + return + end + + %% Angle + phi = acos((trace(R)-1)/2); + + %% Axis + x = [R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)]; + x = x/sqrt(sumsq(x)); + + q = [ cos(phi/2) sin(phi/2)*x]; + +endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/nloscillator.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,132 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {[ @var{dotx}, @var{dotxdx}, @var{u}] =} nloscillator (@var{t}, @var{x}, @var{opt}) +%% Implements a general nonlinear oscillator. +%% @tex +%% $$ +%% \ddot{q} + p(q) + g(\dot{q}) = f(t,q,\dot{q}) +%% $$ +%% @end tex +%% @ifnottex +%% +%% @example +%% q'' + p(q) + g(q') = f(t,q,q') +%% @end example +%% +%% @end ifnottex +%% @noindent +%% where q is the configuration of the system and p(q), g(q') are homogeneous +%% polynomials of arbitrary degree. +%% @tex +%% $$ +%% p(q) = \sum_{i=1}^P a_i q^i, \quad g(\dot{x}) = \sum_{i=1}^G b_i \dot{q}^i +%% $$ +%% @end tex +%% @ifnottex +%% +%% @example +%% @group +%% P +%% p(x) = sum a_i q^i, +%% i=1 +%% G +%% g(x') = sum b_i (q')^i, +%% i=1 +%% @end group +%% @end example +%% +%% @end ifnottex +%% +%% This function can be used with the ODE integrators. +%% +%% @strong{INPUTS} +%% +%% @var{t}: Time. It can be a scalar or a vector of length @code{nT}. +%% +%% @var{x}: State space vector. An array of size @code{2xnT}, where @code{nT} is +%% the number of time values given. The first row corresponds to the configurations +%% of the system and the second row to its derivatives with respect to time. +%% +%% @var{opt}: An options structure. See the complementary function +%% @code{setnloscillator}. The structure containing the fields: +%% +%% @code{Coefficients}: Contains a vector of coefficients for p(x). It follows +%% the format used for function ppval @code{opt.Coefficients(i) = a(P+1-i)}. +%% +%% @code{Damping}: Contains a vector of the coefficients for g(x'). Same format +%% as before. +%% +%% @code{Actuation}: An optional field of the structure. If it is present, it +%% defines the function f(t,q,q'). It can be a handle to a function of the form f = +%% func(@var{t}, @var{x}, @var{opt}) or it can be a @code{1xnT} vector. +%% +%% @strong{OUTPUT} +%% +%% @var{dotx}: Derivative of the state space vector with respect to time. A @code{2xnT} array. +%% +%% @var{dotxdx}: When requested, it contains the Jacobian of the system. It is a +%% multidimensional array of size @code{2x2xnT}. +%% +%% @var{u}: If present, the function returns the inputs that generate the +%% sequence of state space vectors provided in @var{x}. To do this the functions +%% estimates the second derivative of q using spline interpolation. This implies +%% that there be at least 2 observations of the state vector @var{x}, i.e. @code{nT +%% >= 2}. Otherwise the output is empty. +%% +%% @seealso{setnloscillator, ppval, odepkg} +%% @end deftypefn + +function [dotx dotxdx f] = nloscillator (t, x, opt) + + coef = -[opt.Coefficients 0]; + + damp = -[opt.Damping 0]; + + dotx = zeros (size (x)); + + dotx(1,:) = x(2,:); + ac = polyval (coef, x(1,:)) + polyval (damp, x(2,:)); + + F= zeros (1, size (x,2)); + + if isfield (opt, 'Actuation') + F = opt.Actuation; + if ~isnumeric (F); + F = F (t, x, opt); + end + end + + dotx(2,:) = ac + F; + + if nargout > 1 + nt = size (x, 2); + dotxdx = zeros (2, 2, nt); + dcoef = polyder (coef); + ddamp = polyder (damp); + + dotxdx(1,:,:) = [ zeros(1,nt); ones(1,nt) ]; + dotxdx(2,:,:) = [ polyval(dcoef, x(1,:)); polyval(ddamp, x(2,:)) ]; + end + + f = []; + if size (x,2) >= 2 && nargout > 2 + %% inverse dynamics + aci = ppval (ppder (spline (t, x(2,:))), t); + f = aci' - ac; + end + +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/pendulum.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,103 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {[ @var{dotx}, @var{dotxdx}, @var{u}] =} pendulum (@var{t}, @var{x}, @var{opt}) +%% Implements a general pendulum. +%% @tex +%% $$ +%% \ddot{q} + \frac{g}{l}\sin(q) + d\dot{q} = f(t,q,\dot{q}) +%% $$ +%% @end tex +%% @ifnottex +%% +%% @example +%% q'' + (g/l)*sin(q) + d*q' = f(t,q,q') +%% @end example +%% +%% @end ifnottex +%% @noindent +%% where q is the angle of the pendulum and q' its angular velocity +%% +%% This function can be used with the ODE integrators. +%% +%% @strong{INPUTS} +%% +%% @var{t}: Time. It can be a scalar or a vector of length @code{nT}. +%% +%% @var{x}: State space vector. An array of size @code{2xnT}, where @code{nT} is +%% the number of time values given. The first row corresponds to the configurations +%% of the system and the second row to its derivatives with respect to time. +%% +%% @var{opt}: An options structure. See the complementary function +%% @code{setpendulum}. The structure containing the fields: +%% +%% @code{Coefficients}: Contains the coefficients (g/l). +%% +%% @code{Damping}: Contains the coefficient d. +%% +%% @code{Actuation}: An optional field of the structure. If it is present, it +%% defines the function f(t,q,q'). It can be a handle to a function of the form f = +%% func(@var{t},@var{x},@var{opt}) or it can be a @code{1xnT} vector. +%% +%% @strong{OUTPUT} +%% +%% @var{dotx}: Derivative of the state space vector with respect to time. A @code{2xnT} array. +%% +%% @var{dotxdx}: When requested, it contains the Jacobian of the system. It is a multidimensional array of size @code{2x2xnT}. +%% +%% @var{u}: If present, the function returns the inputs that generate the +%% sequence of state space vectors provided in @var{x}. To do this the functions +%% estimates the second derivative of q using spline interpolation. This implies +%% that there be at least 2 observations of the state vector @var{x}, i.e. @code{nT +%% >= 2}. Otherwise the output is empty. +%% +%% @seealso{setpendulum, nloscillator, odepkg} +%% @end deftypefn + +function [dotx dotxdx f] = pendulum (t, x, opt) + + gl = opt.Coefficients(1); + damp = opt.Damping(1); + + dotx = zeros (size (x)); + + dotx(1,:) = x(2,:); + ac = -gl*sin (x(1,:)) - damp*x(2,:); + + F= zeros (1, size (x, 2)); + if isfield (opt, 'Actuation') + F = opt.Actuation; + if ~isnumeric (F); + F = F(t, x, opt); + end + end + + dotx(2,:) = ac + F; + + if nargout > 1 + nt = size (x, 2); + dotxdx = zeros (2, 2, nt); + + dotxdx(1,:,:) = [zeros(1, nt); ones(1, nt)]; + dotxdx(2,:,:) = [-gl*cos(x(1,:)); -damp*ones(1, nt)]; + end + + if size (x, 2) >= 2 && nargout > 2 + %% inverse dynamics + aci = ppval (ppder (spline (t, x(2,:))), t); + f = aci' - ac; + end +endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/principalaxes.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,96 @@ +## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program; if not, see <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {[@var{axes} @var{l} @var{moments}] =} principalaxes (@var{shape}) +## Calculates the principal axes of a shape. +## +## Returns a matrix @var{axes} where each row corresponds to one of the principal +## axes of the shape. @{l} is the second moment of area around the correspoding +## principal axis. @var{axes} is order from lower to higher @var{l}. +## +## @seealso{inertiamoment, masscenter} +## @end deftypefn + +function [PA l Jm] = principalaxes (shape) + + Jm = shapemoment (shape); + + Jsq = Jm(2)^2; + if Jsq > eps; + + TrJ = Jm(1) + Jm(3); + DetJ = Jm(1)*Jm(3) - Jsq; + + %% Eigenvalues + l = ( [TrJ; TrJ] + [1; -1]*sqrt(TrJ^2 - 4*DetJ) )/2; + + %% Eginevectors (Exchanged Jx with Jy) + PA(:,1) = (l - Jm(1)) .* (l - Jm(3)) / Jsq; + PA(:,2) = (l - Jm(1)) .* (l - Jm(3)).^2 / Jm(2)^3; + + %% Normalize + PAnorm = sqrt ( sumsq(PA,2)); + PA(1,:) = PA(1,:) ./ PAnorm(1); + PA(2,:) = PA(2,:) ./ PAnorm(2); + else + + %% Matrix already diagonal + PA(:,1) = [1 ; 0]; + PA(:,2) = [0 ; 1]; + l = [Jm(3); Jm(1)]; + + end + + %% First axis is the one with lowest moment + [l ind] = sort (l, 'ascend'); + PA = PA(ind([2 1]),:); + + %% Check that is a right hand oriented pair of axis + if PA(1,1)*PA(2,2) - PA(1,2)*PA(2,1) < 0 + PA(1,:) = -PA(1,:); + end +end + +%!test +%! h = 1; b = 2; +%! rectangle = [-b/2 -h/2; b/2 -h/2; b/2 h/2; -b/2 h/2]; +%! [PA l] = principalaxes(rectangle); +%! assert ( [0 1; 1 0], PA, 1e-6); +%! assert ([b*h^3; h*b^3]/12, l); + +%!demo +%! t = linspace(0,2*pi,64).'; +%! shape = [cos(t)-0.3*cos(3*t) sin(t)](1:end-1,:); +%! shapeR = shape*rotv([0 0 1],pi/4)(1:2,1:2); +%! [PAr l] = principalaxes(shapeR); +%! [PA l] = principalaxes(shape); +%! +%! cla +%! plot(shape(:,1),shape(:,2),'-k'); +%! line([0 PA(1,1)],[0 PA(1,2)],'color','r'); +%! line([0 PA(2,1)],[0 PA(2,2)],'color','b'); +%! +%! hold on +%! +%! plot(shapeR(:,1)+3,shapeR(:,2),'-k'); +%! line([3 PAr(1,1)+3],[0 PAr(1,2)],'color','r'); +%! line([3 PAr(2,1)+3],[0 PAr(2,2)],'color','b'); +%! +%! axis equal +%! axis square + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/quat2mat.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,7 @@ +function R = quat2mat (q) + +R = [ q(1)^2+q(2)^2-q(3)^2-q(4)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4) + q(1)*q(3)); ... + 2*(q(2)*q(3)+q(1)*q(4)) q(1)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4) - q(1)*q(2)); ... + 2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)^2-q(2)^2-q(3)^2+q(4)^2 ]; + +endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/quatconj.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,23 @@ +%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: youcan redistribute itand/or modify +%% it under the terms of the GNU General Public Licenseas publishedby +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it willbe useful, +%% but WITHOUTaNY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have receivedacopy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{qc} = } quatconj (@var{q}) +%% Conjugate of a quaternion. +%% @end deftypefn + +function qc = quatconj(q); + qc = [q(:,1) -q(:,2:4)]; +endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/quatprod.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,29 @@ +%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: youcan redistribute itand/or modify +%% it under the terms of the GNU General Public Licenseas publishedby +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it willbe useful, +%% but WITHOUTaNY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have receivedacopy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{q3} = } quatprod (@var{q1}, @var{q2}) +%% Product of two quaternions. +%% @end deftypefn + +function q3 = quatprod (q1,q2) + +q3 = [ q1(:,1).*q2(:,1) - q1(:,2).*q2(:,2) - q1(:,3).*q2(:,3) - q1(:,4).*q2(:,4) ... + q1(:,1).*q2(:,2) + q1(:,2).*q2(:,1) + q1(:,3).*q2(:,4) - q1(:,4).*q2(:,3) ... + q1(:,1).*q2(:,3) - q1(:,2).*q2(:,4) + q1(:,3).*q2(:,1) + q1(:,4).*q2(:,2) ... + q1(:,1).*q2(:,4) + q1(:,2).*q2(:,3) - q1(:,3).*q2(:,2) + q1(:,4).*q2(:,1) ]; + +endfunction +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/quatvrot.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,23 @@ +%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: youcan redistribute itand/or modify +%% it under the terms of the GNU General Public Licenseas publishedby +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it willbe useful, +%% but WITHOUTaNY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have receivedacopy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{vn} = } quatvrot (@var{v}, @var{q}) +%% Rotate vector v accoding to quaternionr q. +%% @end deftypefn + +function rn = quatvrot(r,q) + rn = quatprod(quatprod(q,[zeros(size(r,1),1) r]),quatconj(q))(:,2:end); +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/setnloscillator.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,47 @@ +%% Copyright (c) 2010 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {[ @var{opts}, @var{desc}] =} setnloscilator () +%% Returns the required options structure for the function nloscillator and a +%% description of the fields in the structure. +%% +%% @seealso{nloscillator} +%% @end deftypefn + +function [opt desc] = setnloscillator(varargin) + +required_fields = {'Coefficients','Damping'}; +reqf_default = {[0.5 0 1];0.01}; +reqf_description = {['Coefficients for the spring polynomial without ' ... + 'homogeneous part. The number of elements is the degree ' ... + 'of the polynomial. the first element is the leading coefficient.'];... + ['Coefficients for the damping polynomial without ' ... + 'homogeneous part. The number of elements is the degree ' ... + 'of the polynomial. the first element is the leading coefficient.']}; + +optional_fields = {'Actuation'}; +optf_description = {['Optional field. It defines the forcing function (source)'... + 'f(t). It can be a handle to a function of the form f = func(t,x,opt)' ... + 'or it can be a 1xnT array.']}; + +opt = cell2struct(reqf_default,required_fields,1); + +if nargout > 1 + cell1 = {reqf_description{:},optf_description{:}}'; + cell2 = {required_fields{:},optional_fields{:}}; + desc = cell2struct(cell1,cell2,1); +end +endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/core/setpendulum.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,43 @@ +%% Copyright (c) 2010 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {[ @var{opts}, @var{desc}] =} setpendulum () +%% Returns the required options structure for the function pendulum and a +%% description of the fields in the structure. +%% +%% @seealso{pendulum} +%% @end deftypefn + +function [opt desc] = setpendulum(varargin) + +required_fields = {'Coefficients','Damping'}; +reqf_default = {1;0}; +reqf_description = {['Ration (g/l), relation between gravity and length of the pendulum'];... + ['Damping coefficient, damping is proportional to angular speed.']}; + +optional_fields = {'Actuation'}; +optf_description = {['Optional field. It defines the forcing function (source)'... + "f(t,q,q'). It can be a handle to a function of the form f = func(t,x,opt)" ... + 'or it can be a 1xnT array.']}; + +opt = cell2struct(reqf_default,required_fields,1); + +if nargout > 1 + cell1 = {reqf_description{:},optf_description{:}}'; + cell2 = {required_fields{:},optional_fields{:}}; + desc = cell2struct(cell1,cell2,1); +end +endfunction
--- a/main/mechanics/inst/drawAxis3D.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,21 +0,0 @@ -%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: youcan redistribute itand/or modify -%% it under the terms of the GNU General Public Licenseas publishedby -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it willbe useful, -%% but WITHOUTaNY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have receivedacopy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -function drawAxis3D (p,E,c) - for i=1:3 - l =[p; p+E(i,:)]; - line(l(:,1),l(:,2),l(:,3),'color',c(i,:),'linewidth',2); - end -end
--- a/main/mechanics/inst/forcematrix.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,204 +0,0 @@ -%% Copyright (c) 2010 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: you can redistribute it and/or modify -%% it under the terms of the GNU General Public License as published by -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it will be useful, -%% but WITHOUT ANY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have received a copy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {[@var{F}, @var{D}] =} forcematrix (@var{pos}, @var{A},@var{funcs}) -%% Evaluates a force function depending on the separation of multiple interacting points. -%% -%% @strong{INPUT} -%% -%% @itemize -%% @item -%% @var{pos} is a Nxdim array of positions. N is the number of points and dim the -%% dimension. Each row represent a different point. -%% -%% @item -%% @var{A} is the a connectivity (adjacency) matrix expressed as a vector. That is, if -%% @var{M} is the complete NxN symmetric connectivity matrix, then -%% @code{@var{A}= vech(@var{M})}. The diagonal of @var{M} is not used. The elements -%% of A are indexes to the corresponding interaction force in @var{funcs}. For -%% example, there are 3 points and points 2 and 3 interact with a force -%% described in the 5th element of @var{funcs}, then @code{ M(2, 3) = 5}. -%% -%% @item -%% @var{funcs} is a Nfx1 cell. Elements that are matrices are evaluated with -%% @code{polyval}. elements that are function handles are evaluated directly. -%% Functions handles must be of the form @code{@@(@var{x})f(@var{x})}, where x is -%% the distance between two interacting points. -%% -%% @end itemize -%% -%% @strong{OUTPUT} -%% -%% @itemize -%% @item -%% @var{F} is a Nxdim array of forces acting on each point. -%% -%% @item -%% @var{D} is an array in the same format as @var{A} containing the euclidean -%% distances between the points. -%% @end itemize -%% -%% @html -%% @include doc/matrixforce.svg -%% @end html -%% @seealso{pointmassmesh, polyval, vech, sub2ind} -%% @end deftypefn - -function [F D] = forcematrix (pos, A, funcs) - - %% Argument parsing - [N dim] = size(pos); - nA = numel(A); - - % Relative position matrix - dr = zeros (nA,dim); - for ic = 1:dim - dr(:,ic) = vech (bsxfun (@minus, pos(:,ic)', pos(:,ic))); - end - % Distance vector - D = sqrt (sum (dr.^2, 2)); - % Versor - drhat = dr ./ repmat(D, 1,dim); - - %% Extend funcs to handle 0 elements. This or eliminate all diagonals. - funcs{end+1} = 0; - nf = numel(funcs); - A(A==0) = nf; - - %% pair to pair forces - f = cellfun (@handleOpoly, {funcs{A}}.', mat2cell (D, ones(nA,1), 1)); - f = repmat(f,1,dim) .* drhat; - - %% net force on p1 - F = zeros (N,dim); - - %% FIXME avoid looping - aux = ones(1,N-1); - p2 = sub2ind_tril (N, 2:N, aux); - F(1,:) = sum (f(p2,:)); - - p2 = sub2ind_tril (N, 1:N-1, N*aux ); - F(N,:) = -sum (f(p2,:)); - - for p1 = 2:N-1 - idx = sub2ind_tril (N, 1:N, repmat (p1,1,N) ); - sthalf = idx(1:(p1-1)); - ndhalf = idx((p1+1):N); - F(p1,:) = sum (f(ndhalf,:)) - sum (f(sthalf,:)); - end - -endfunction - -function f = handleOpoly (y,x) - - if isnumeric(y) - f = polyval(y,x); - else - f = y(x); - end - -end - -%% 1D -%!test -%! pos = [0;1]; A=vech ([0 1; 1 0]); funcs={[-1 1]}; -%! [F D] = forcematrix (pos, A, funcs); -%! assert (F, [0; 0], 1e-8); -%! assert (D, [0; 1; 0]); - -%!test -%! pos = [0;1;2]; A=vech ([0 1 0; 1 0 1; 0 1 0]); funcs={[-1 1]}; -%! [F D] = forcematrix (pos, A, funcs); -%! assert (F, [0; 0; 0], 1e-8); -%! assert (D, [0; 1; 2; 0; 1; 0]); - -%!test -%! pos = [0;1;2]; A=vech ([0 1 0; 1 0 1; 1 0 0]); funcs={[-1 1]}; -%! [F D] = forcematrix (pos, A, funcs); -%! assert (F, [1; 0; -1], 1e-8); -%! assert (D, [0; 1; 2; 0; 1; 0]); - -%!test -%! pos = [0;1;2]; A=vech ([0 1 0; 1 0 1; 1 1 0]); funcs={[-1 1]}; -%! [F D] = forcematrix (pos, A, funcs); -%! assert (F, [1; 0; -1], 1e-8); -%! assert (D, [0; 1; 2; 0; 1; 0]); - -%% 2D -%!test % Equilateral triangle -%! pos = [0 0;1 0;cos(pi/3) sin(pi/3)]; A=[0;1;1; 0;1; 0]; funcs={[-1 1]}; -%! [F D] = forcematrix (pos, A, funcs); -%! assert (F, [0 0; 0 0; 0 0], 1e-8); -%! assert (D, [0; 1; 1; 0; 1; 0], 1e-8); - -%!test % Square -%! pos = [0 0;1 0;1 1;0 1]; A=[0;1;0;1; 0;1;0; 0;1; 0]; funcs={[-1 1]}; -%! [F D] = forcematrix (pos, A, funcs); -%! assert (F, [0 0; 0 0; 0 0; 0 0], 1e-8); -%! assert (D, [0;1;sqrt(2);1; 0;1;sqrt(2); 0;1; 0], 1e-8); - - -% 3D -%!test % Cube -%! pos = [0 0 0; 1 0 0; 1 1 0; 0 1 0; ... -%! 0 0 1; 1 0 1; 1 1 1; 0 1 1;]; -%! A= [0;1;0;1;1;0;0;0; ... -%! 0;1;0;0;1;0;0; ... -%! 0;1;0;0;1;0; ... -%! 0;0;0;0;1; ... -%! 0;1;0;1; ... -%! 0;1;0; ... -%! 0;1; ... -%! 0; ]; -%! funcs={[-1 1]}; -%! [F D] = forcematrix (pos, A, funcs); -%! assert (F, zeros(8,3), 1e-8); -%! D_sb = [0;sqrt([1;2;1;1;2;3;2]); ... -%! 0;sqrt([1;2;2;1;2;3]); ... -%! 0;sqrt([1;3;2;1;2]); ... -%! 0;sqrt([2;3;2;1]); ... -%! 0;sqrt([1;2;1]); ... -%! 0;sqrt([1;2]); ... -%! 0;sqrt(1); ... -%! 0]; -%! assert (D, D_sb, 1e-8); - -%!test % Cube 2 funcs -%! pos = [0 0 0; 1 0 0; 1 1 0; 0 1 0; ... -%! 0 0 1; 1 0 1; 1 1 1; 0 1 1;]; -%! A= [0;1;0;1;2;0;0;0; ... -%! 0;1;0;0;2;0;0; ... -%! 0;1;0;0;2;0; ... -%! 0;0;0;0;2; ... -%! 0;1;0;1; ... -%! 0;1;0; ... -%! 0;1; ... -%! 0; ]; -%! funcs={[-1 1], @(x)-x}; -%! [F D] = forcematrix (pos, A, funcs); -%! F_sb = [zeros(4,2) ones(4,1); ... -%! zeros(4,2) -ones(4,1)]; -%! assert (F, F_sb, 1e-8); -%! D_sb = [0;sqrt([1;2;1;1;2;3;2]); ... -%! 0;sqrt([1;2;2;1;2;3]); ... -%! 0;sqrt([1;3;2;1;2]); ... -%! 0;sqrt([2;3;2;1]); ... -%! 0;sqrt([1;2;1]); ... -%! 0;sqrt([1;2]); ... -%! 0;sqrt(1); ... -%! 0]; -%! assert (D, D_sb, 1e-8); -
--- a/main/mechanics/inst/inertiamoment.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,75 +0,0 @@ -%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software; you can redistribute it and/or modify -%% it under the terms of the GNU General Public License as published by -%% the Free Software Foundation; either version 3 of the License, or -%% (at your option) any later version. -%% -%% This program is distributed in the hope that it will be useful, -%% but WITHOUT ANY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have received a copy of the GNU General Public License -%% along with this program; if not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} { @var{Iz} =} inertiamoment (@var{pp}, @var{m}) -%% Moment of intertia of a plane shape. -%% -%% Calculates the moment of inertia respect to an axis perpendicular to the -%% plane passing through the center of mass of the body. -%% -%% The shape is defined with piecewise smooth polynomials. @var{pp} is a -%% cell where each elements is a 2-by-(poly_degree+1) matrix containing px(i,:) = -%% pp{i}(1,:) and py(i,:) = pp{i}(2,:). -%% -%% @seealso{masscenter, principalaxis} -%% @end deftypefn - -function Iz = inertiamoment (pp, m) - - Iz = sum(cellfun (@Iint, pp)); - A = shapearea(pp); - Iz = m*Iz / A; - -endfunction - -function dI = Iint (x) - - px = x(1,:); - py = x(2,:); - - paux = conv (px, px)/3 + conv(py, py); - paux2 = conv (px, polyderiv (py)) ; - P = polyint (conv (paux, paux2)); - - %% Faster than quad - dI = diff(polyval(P,[0 1])); - -endfunction - -%!demo % non-convex bezier shape -%! weirdhearth ={[-17.6816 -34.3989 7.8580 3.7971; ... -%! 15.4585 -28.3820 -18.7645 9.8519]; ... -%! [-27.7359 18.1039 -34.5718 3.7878; ... -%! -40.7440 49.7999 -25.5011 2.2304]}; -%! Iz = inertiamoment (weirdhearth,1) - -%!test -%! square = {[1 -0.5; 0 -0.5]; [0 0.5; 1 -0.5]; [-1 0.5; 0 0.5]; [0 -0.5; -1 0.5]}; -%! Iz = inertiamoment (square,1); -%! assert (1/6, Iz, sqrt(eps)); - -%!test -%! circle = {[1.715729 -6.715729 0 5; ... -%! -1.715729 -1.568542 8.284271 0]; ... -%! [1.715729 1.568542 -8.284271 0; ... -%! 1.715729 -6.715729 0 5]; ... -%! [-1.715729 6.715729 0 -5; ... -%! 1.715729 1.568542 -8.284271 0]; ... -%! [-1.715729 -1.568542 8.284271 0; ... -%! -1.715729 6.715729 0 -5]}; -%! Iz = inertiamoment (circle,1); -%! assert (Iz, 0.5*5^2, 5e-3); -
--- a/main/mechanics/inst/masscenter.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,71 +0,0 @@ -%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software; you can redistribute it and/or modify -%% it under the terms of the GNU General Public License as published by -%% the Free Software Foundation; either version 3 of the License, or -%% (at your option) any later version. -%% -%% This program is distributed in the hope that it will be useful, -%% but WITHOUT ANY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have received a copy of the GNU General Public License -%% along with this program; if not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} { @var{cm} =} masscenter (@var{pp}) -%% Center of mass of a plane shape. -%% -%% The shape is defined with piecewise smooth polynomials. @var{pp} is a -%% cell where each elements is a 2-by-(poly_degree+1) matrix containing px(i,:) = -%% pp{i}(1,:) and py(i,:) = pp{i}(2,:). -%% -%% @seealso{inertiamoment} -%% @end deftypefn - -function cm = masscenter (shape) - - cm = sum( cell2mat ( cellfun (@CMint, shape, 'UniformOutput', false))); - A = shapearea(shape); - cm = cm / A; - -endfunction - -function dcm = CMint (x) - - px = x(1,:); - py = x(2,:); - Px = polyint (conv(conv (px , px)/2 , polyderiv (py))); - Py = polyint (conv(-conv (px , px)/2 , polyderiv (px))); - - dcm = zeros (1,2); - dcm(1) = diff(polyval(Px,[0 1])); - dcm(2) = diff(polyval(Py,[0 1])); - -endfunction - -%!demo % non-convex bezier shape -%! weirdhearth ={[-17.6816 -34.3989 7.8580 3.7971; ... -%! 15.4585 -28.3820 -18.7645 9.8519]; ... -%! [-27.7359 18.1039 -34.5718 3.7878; ... -%! -40.7440 49.7999 -25.5011 2.2304]}; -%! CoM = masscenter (weirdhearth) - -%!test -%! square = {[1 -0.5; 0 -0.5]; [0 0.5; 1 -0.5]; [-1 0.5; 0 0.5]; [0 -0.5; -1 0.5]}; -%! CoM = masscenter (square); -%! assert (CoM, [0 0], sqrt(eps)); - -%!test -%! circle = {[1.715729 -6.715729 0 5; ... -%! -1.715729 -1.568542 8.284271 0]; ... -%! [1.715729 1.568542 -8.284271 0; ... -%! 1.715729 -6.715729 0 5]; ... -%! [-1.715729 6.715729 0 -5; ... -%! 1.715729 1.568542 -8.284271 0]; ... -%! [-1.715729 -1.568542 8.284271 0; ... -%! -1.715729 6.715729 0 -5]}; -%! CoM = masscenter (circle); -%! assert (CoM , [0 0], 5e-3); -
--- a/main/mechanics/inst/mat2quat.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,17 +0,0 @@ -function q = mat2quat(R) - - if all(R == eye(3)) - q = [1 0 0 0]; - return - end - - %% Angle - phi = acos((trace(R)-1)/2); - - %% Axis - x = [R(3,2)-R(2,3) R(1,3)-R(3,1) R(2,1)-R(1,2)]; - x = x/sqrt(sumsq(x)); - - q = [ cos(phi/2) sin(phi/2)*x]; - -endfunction
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/inst/demofunc1.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,26 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{f1},@var{f2}} = demofunc1 (@var{dx},@var{dv}) +%% function used in demos. +%% +%% @end deftypefn + +function [f1 f2] = demofunc1 (dx, dv) + f1 = lennardjones (dx, (0.15)^2, 1e-2); + f2 = -f1; +endfunction +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/inst/demofunc2.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,26 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{f1},@var{f2}} = demofunc2 (@var{x1},@var{x2},@var{v1},@var{v2}) +%% function used in demos. +%% +%% @end deftypefn + +function [f1 f2] = demofunc2 (x1,x2,v1,v2) + f1 = -15 *((x1-x2) + 0.2) -0.7*(v1-v2); + f2 = -f1; +end +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/inst/demofunc3.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,26 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{f1},@var{f2}} = demofunc3 (@var{dx},@var{dv}) +%% function used in demos. +%% +%% @end deftypefn + +function [f1 f2] = demofunc3 (dx, dv) + f1 = lennardjones (dx, (0.2)^2, 1); + f2 = -f1; + end +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/inst/lennardjones.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,34 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{f},@var{v}} = lennardjones (@var{dx},@var{r02},@var{v0}) +%% Returns force and energy of the Lennard-Jonnes potential evaluated at @var{dx}. +%% +%% @end deftypefn + +function [f V] = lennardjones (dx,r02,V0) + + r2 = sumsq(dx,2); + dr = dx ./ r2; + + rr3 = (r02 ./ r2).^3; + rr6 = rr3.^2; + + V = V0 * ( rr6 - 2*rr3 ); + + f = bsxfun(@times, 12 * V0 * ( rr6 - rr3 ), dr); + +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/inst/mdsim.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,220 @@ +%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> +%% +%% This program is free software: you can redistribute it and/or modify +%% it under the terms of the GNU General Public License as published by +%% the Free Software Foundation, either version 3 of the License, or +%% any later version. +%% +%% This program is distributed in the hope that it will be useful, +%% but WITHOUT ANY WARRANTY; without even the implied warranty of +%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +%% GNU General Public License for more details. +%% +%% You should have received a copy of the GNU General Public License +%% along with this program. If not, see <http://www.gnu.org/licenses/>. + +%% -*- texinfo -*- +%% @deftypefn {Function File} {@var{t},@var{pos},@var{vel}} = mdsim (@var{func},@var{tspan},@var{p0},@var{v0}) +%% Integrates a system of particles using velocity Verlet algorithm. +%% +%% @end deftypefn + +function [tspan y vy] = mdsim(func,tspan,x0,v0,varargin); + + %% Parse options + options = {"mass","timestep","nonperiodic","substeps","boxlength"}; + opt_given = {varargin{1:2:end}}; + [tf idx] = ismember (options, opt_given); + + % Parse masses + m = ones (size (x0,1), 1); + if tf(1) + m = varargin{idx(1)+1}; + end + + % Parse time step & substeps + nT = numel (tspan); + if nT == 1 + tspan(2)=tspan; + tspan(1)=0; + end + + if tf(2) && tf(4) + error('mdsim:Error','You cannot define substeps and dt simultaneously.\n'); + elseif tf(2) && ~tf(4) + + dt = varargin{idx(2)+1}; + if ~isscalar (dt) || any (dt < 0); + error('mdsim:Error','Timestep must be a positive scalar.\n'); + end + + substep = round( (tspan(2)-tspan(1)) / dt); + + elseif ~tf(2) && tf(4) + + substep = varargin{idx(4)+1}; + if ~isscalar (substep) || any (substep < 2); + error('mdsim:Error','Substeps must be a natural number >= 2.\n'); + end + + dt = (tspan(2)-tspan(1)) / (substep-1); + + else + substep = 1e1; + dt = (tspan(2)-tspan(1)) / (substep-1); + end + + % Parse periodic bc or not + L = 1; + if tf(5) + L = varargin{idx(5)+1}; + if ~isscalar (L) || any (L < 0); + error('mdsim:Error','Length of box side should be a positive scalar.\n'); + end + end + if nargout(func) < 2 && nargin(func) < 4 + error('mdsim:Error',['Interaction force must accept at least 4 arguments'... + 'and return at least 2, when boundary conditions are periodic.\n']); + end + integrator = @(x_,v_,dt_) verletstep_boxed (x_, v_, m, dt_, func, L); + + if tf(3) + if nargout(func) < 2 && nargin(func) < 2 + error('mdsim:Error',['Interaction force must accept at least 2' ... + 'arguments\nwhen boundary conditions are not periodic.\n']); + end + integrator = @(x_,v_,dt_) verletstep (x_, v_, m, dt_, func); + end + + %% Allocate + [N dim] = size(x0); + y = zeros (N, dim, nT); + + if nargout > 2 + vy = y; + vy(:,:,1) = v0; + end + + %% Iterate ------------ + if tf(3) + y(:,:,1) = x0; + auxX = x0; + auxV = v0; + else + auxX = x0; + ind = find(x0 < -L/2); + auxX(ind) = auxX(ind) + L * abs (round (auxX(ind)/L)); + ind = find(x0 > -L/2); + auxX(ind) = auxX(ind) - L * abs (round (auxX(ind)/L)); + y(:,:,1) = auxX; + auxV = v0; + end + + for it = 2:nT + + for jt = 1:substep + [auxX auxV] = integrator (auxX, auxV, dt); + end + + y(:,:,it) = auxX; + if nargout > 2 + vy(:,:,it) = auxV; + end + + end + + if dim == 1 + y = squeeze (y); + if nargout > 2 + vy = squeeze (vy); + end + end + +endfunction + +% Test arguments +%!function [fx fy] = func4 (xq, x2, v1, v2) +%! fx = 0; fy = 0; +%! end + +%!function [fx fy] = func2 (dx, dv) +%! fx = 0; fy = 0; +%! end + +%!error (mdsim (@func2, [0 1], 0, 0, "nonperiodic", false)) +%!error (mdsim (@func4, [0 1], 0, 0, "nonperiodic", true)) +%!error (mdsim (@func4, [0 1], 0, 0)) +%!error (mdsim (@func2, [0 1], 0, 0,"timestep",-1)) +%!error (mdsim (@func2, [0 1], 0, 0,"timestep",[0 1])) +%!error (mdsim (@func2, [0 1], 0, 0,"timestep",1,"substep",2)) +%!error (mdsim (@func2, [0 1], 0, 0,"substep",0)) +%!error (mdsim (@func2, [0 1], 0, 0,"substep",[0 1])) +%!error (mdsim (@func2, [0 1], 0, 0,"boxlength",-1)) +%!error (mdsim (@func2, [0 1], 0, 0,"boxlength",[1 2])) +%!error (mdsim ([], [0 1], 0, 0,"boxlength",[1 2])) + +%!demo +%! N = 6; +%! P0 = linspace (-0.5+1/(N-1), 0.5-1/(N-1), N).'; +%! V0 = zeros (N, 1); +%! nT = 80; +%! tspan = linspace(0, 2, nT); +%! [t P V] = mdsim ('demofunc1', tspan, P0, V0,'timestep',1e-3); +%! +%! figure (1) +%! plot (P.',t,'.'); +%! xlabel ("Position") +%! ylabel ("Time") +%! axis tight +%! +%! disp("Initial values") +%! disp ( sum ([V(:,1) P(:,1)], 1) ) +%! disp("Final values") +%! disp ( sum ([V(:,end) P(:,end)], 1) ) +%! +%! %------------------------------------------------------------------- +%! % 1D particles with Lennard-Jones potential and periodic boundaries. +%! % Velocity and position of the center of mass are conserved. + +%!demo +%! N = 10; +%! P0 = linspace (0,1,N).'; +%! V0 = zeros (N, 1); +%! nT = 80; +%! tspan = linspace(0, 1, nT); +%! [t P] = mdsim ('demofunc2', tspan, P0, V0,'nonperiodic', true); +%! +%! figure (1) +%! plot (P.',t,'.'); +%! xlabel ("Position") +%! ylabel ("Time") +%! +%! %------------------------------------------------------------------- +%! % 1D array of springs with damping proportional to relative velocity and +%! % nonzero rest length. + + +%!demo +%! +%! input("NOTE: It may take a while.\nPress Ctrl-C to cancel or <enter> to continue: ","s"); +%! +%! N = 4; +%! [Px Py] = meshgrid (linspace (-0.5+0.5/(N-1), 0.5-0.5/(N-1), N)); +%! P0 = [Px(:) Py(:)]; +%! N = size(P0,1); +%! P0 = P0 + 0.1* 0.5/N *(2*rand (size (P0)) - 1); +%! V0 = zeros (N, 2); +%! nT = 80; +%! tspan = linspace(0, 1, nT); +%! [t P] = mdsim ('demofunc3', tspan, P0, V0); +%! x = squeeze(P(:,1,:)); +%! y = squeeze(P(:,2,:)); +%! +%! figure (1) +%! plot (x.',y.','.',x(:,end),y(:,end),'.k'); +%! xlabel ("X") +%! ylabel ("Y") +%! axis tight +%! +%! %------------------------------------------------------------------- +%! % 2D particles with Lennard-Jones potential and periodic boundaries
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/src/Makefile Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,7 @@ +all: verletstep.oct verletstep_boxed.oct + +%.oct: %.cc + mkoctfile -Wall $< + +clean: + rm -f *.o octave-core core *.oct *~
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/src/verletstep.cc Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,113 @@ +#include <octave/oct.h> +#include <octave/parse.h> + +DEFUN_DLD (verletstep, args, nargout, "Verlet velocity step") + { + int nargin = args.length(); + octave_value_list retval; + + unsigned int fcn_str = 0; + + if (nargin < 5) + print_usage (); + else + { + // Allocate inputs + octave_function *fcn; + if (args(4).is_function_handle () || args(4).is_inline_function ()) + fcn_str = 0; + else if (args(4).is_string ()) + fcn_str = 1; + else + error ("verletstep: expected string,"," inline or function handle"); + + Matrix P = args(0).matrix_value (); + Matrix V = args(1).matrix_value (); + Matrix M = args(2).matrix_value (); + double dt = args(3).scalar_value (); + + dim_vector dv = P.dims(); + octave_idx_type Nparticles = dv(0); + octave_idx_type Ndim = dv(1); + + octave_value_list newargs; + Matrix A (dim_vector(Nparticles, Ndim),0); + + // Evaluate interaction function + for (octave_idx_type ipart = 0; ipart < Nparticles; ipart++) + { + newargs(0) = P.row(ipart); + newargs(2) = V.row(ipart); + + for (octave_idx_type jpart = ipart + 1; jpart < Nparticles; jpart++) + { + newargs(1) = P.row(jpart); + newargs(3) = V.row(jpart); + + if (fcn_str) + retval = feval (args (4).string_value (), newargs, nargout); + else + { + fcn = args(4).function_value (); + if (! error_state) + retval = feval (fcn, newargs, nargout); + } + + A.insert (retval(0).row_vector_value () + + A.row (ipart), ipart, 0); + A.insert (retval(1).row_vector_value () + + A.row (jpart), jpart, 0); + + } + } + + for (octave_idx_type i = 0; i < A.rows (); i++) + A.insert (A.row (i) / M(i), i, 0); + + // Integrate by half time step velocity and full step position + V = V + A * dt/2; + P = P + V * dt; + + A.fill(0); + + // Evaluate interaction function + for (octave_idx_type ipart = 0; ipart < Nparticles; ipart++) + { + newargs(0) = P.row(ipart); + newargs(2) = V.row(ipart); + + for (octave_idx_type jpart = ipart + 1; jpart < Nparticles; jpart++) + { + newargs(1) = P.row(jpart); + newargs(3) = V.row(jpart); + + if (fcn_str) + retval = feval (args (4).string_value (), newargs, nargout); + else + { + fcn = args(4).function_value (); + if (! error_state) + retval = feval (fcn, newargs, nargout); + } + + A.insert (retval(0).row_vector_value () + + A.row (ipart), ipart, 0); + A.insert (retval(1).row_vector_value () + + A.row (jpart), jpart, 0); + + } + } + for (octave_idx_type i = 0; i < A.rows (); i++) + A.insert (A.row (i) / M(i), i, 0); + + // Integrate velocity for the rest of the time step + V = V + A * dt/2; + + retval(0) = P; + retval(1) = V; + retval(2) = A; + } + + return retval; + } +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/src/verletstep_boxed.cc Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,135 @@ +#include <octave/oct.h> +#include <octave/parse.h> + +Matrix min_image (Matrix P, double L) +{ + // Returns the image of a particle inside a square box of side L centered in the origin + Matrix Pn = P; + for (octave_idx_type ip = 0; ip < P.rows (); ip++) + for (octave_idx_type dim = 0; dim < P.columns (); dim++) + { + if (P(ip,dim) > L/2) + Pn.fill(P(ip,dim) - L * labs (lrint (P(ip,dim) / L)), ip, dim, ip, dim); + + else if (P(ip,dim) < -L/2) + Pn.fill(P(ip,dim) + L * labs (lrint (P(ip,dim) / L)), ip, dim, ip, dim); + } + return Pn; +} + +/* When this version is used, the interaction function must depend on the + relative position and velocities only. +*/ +DEFUN_DLD (verletstep_boxed, args, nargout, "Verlet velocity step in periodic space") + { + int nargin = args.length(); + octave_value_list retval; + + unsigned int fcn_str = 0; + + if (nargin < 6) + print_usage (); + else + { + // Allocate inputs + octave_function *fcn; + if (args(4).is_function_handle () || args(4).is_inline_function ()) + fcn_str = 0; + else if (args(4).is_string ()) + fcn_str = 1; + else + error ("verletstep: expected string,"," inline or function handle"); + + Matrix P = args(0).matrix_value (); + Matrix V = args(1).matrix_value (); + Matrix M = args(2).matrix_value (); + double dt = args(3).scalar_value (); + + dim_vector dv = P.dims(); + octave_idx_type Nparticles = dv(0); + octave_idx_type Ndim = dv(1); + + octave_value_list newargs; + Matrix A (dim_vector(Nparticles, Ndim),0); + + // Box containing the particles + double L = args(5).scalar_value (); + + // Evaluate interaction function + for (octave_idx_type ipart = 0; ipart < Nparticles; ipart++) + { + + for (octave_idx_type jpart = ipart + 1; jpart < Nparticles; jpart++) + { + newargs(0) = min_image (P.row(ipart) - P.row(jpart), L); + newargs(1) = V.row(ipart) - V.row(jpart); + + if (fcn_str) + retval = feval (args (4).string_value (), newargs, nargout); + else + { + fcn = args(4).function_value (); + if (! error_state) + retval = feval (fcn, newargs, nargout); + } + + A.insert (retval(0).row_vector_value () + + A.row (ipart), ipart, 0); + A.insert (retval(1).row_vector_value () + + A.row (jpart), jpart, 0); + + } + } + + for (octave_idx_type i = 0; i < A.rows (); i++) + A.insert (A.row (i) / M(i), i, 0); + + // Integrate by half time step velocity and full step position + V = V + A * dt/2; + P = P + V * dt; + + A.fill(0); + + // Evaluate interaction function + // Evaluate interaction function + for (octave_idx_type ipart = 0; ipart < Nparticles; ipart++) + { + + for (octave_idx_type jpart = ipart + 1; jpart < Nparticles; jpart++) + { + newargs(0) = min_image (P.row(ipart) - P.row(jpart), L); + newargs(1) = V.row(ipart) - V.row(jpart); + + if (fcn_str) + retval = feval (args (4).string_value (), newargs, nargout); + else + { + fcn = args(4).function_value (); + if (! error_state) + retval = feval (fcn, newargs, nargout); + } + + A.insert (retval(0).row_vector_value () + + A.row (ipart), ipart, 0); + A.insert (retval(1).row_vector_value () + + A.row (jpart), jpart, 0); + + } + } + for (octave_idx_type i = 0; i < A.rows (); i++) + A.insert (A.row (i) / M(i), i, 0); + + // Integrate velocity for the rest of the time step + V = V + A * dt/2; + + // Put all particles in a box of side L + P = min_image (P, L); + + retval(0) = P; + retval(1) = V; + retval(2) = A; + } + + return retval; + } +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/molecularDynamics/src/verletstep_m.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,23 @@ +function [P V F] = verletstep_m(P, V, M, dt, force) + + [Npart Ndims] = size(P); + PP = repmat(P,1,Npart); + VV = repmat(V,1,Npart); + MM = repmat(M,1,Ndims); + + [F1 F2] = arrayfun (force, PP, PP', VV, VV'); + F = sum (triu (F1, 1) + triu (F2, 1).', 2) ./ MM; + + + V = V + F * dt/2; + P = P + V * dt; + + PP = repmat(P,1,Npart); + VV = repmat(V,1,Npart); + + [F1 F2] = arrayfun (force, PP, PP', VV, VV'); + F = sum (triu (F1, 1) + triu (F2, 1).', 2) ./ MM; + + V = V + F * dt/2; + +end
--- a/main/mechanics/inst/nloscillator.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,132 +0,0 @@ -%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: you can redistribute it and/or modify -%% it under the terms of the GNU General Public License as published by -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it will be useful, -%% but WITHOUT ANY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have received a copy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {[ @var{dotx}, @var{dotxdx}, @var{u}] =} nloscillator (@var{t}, @var{x}, @var{opt}) -%% Implements a general nonlinear oscillator. -%% @tex -%% $$ -%% \ddot{q} + p(q) + g(\dot{q}) = f(t,q,\dot{q}) -%% $$ -%% @end tex -%% @ifnottex -%% -%% @example -%% q'' + p(q) + g(q') = f(t,q,q') -%% @end example -%% -%% @end ifnottex -%% @noindent -%% where q is the configuration of the system and p(q), g(q') are homogeneous -%% polynomials of arbitrary degree. -%% @tex -%% $$ -%% p(q) = \sum_{i=1}^P a_i q^i, \quad g(\dot{x}) = \sum_{i=1}^G b_i \dot{q}^i -%% $$ -%% @end tex -%% @ifnottex -%% -%% @example -%% @group -%% P -%% p(x) = sum a_i q^i, -%% i=1 -%% G -%% g(x') = sum b_i (q')^i, -%% i=1 -%% @end group -%% @end example -%% -%% @end ifnottex -%% -%% This function can be used with the ODE integrators. -%% -%% @strong{INPUTS} -%% -%% @var{t}: Time. It can be a scalar or a vector of length @code{nT}. -%% -%% @var{x}: State space vector. An array of size @code{2xnT}, where @code{nT} is -%% the number of time values given. The first row corresponds to the configurations -%% of the system and the second row to its derivatives with respect to time. -%% -%% @var{opt}: An options structure. See the complementary function -%% @code{setnloscillator}. The structure containing the fields: -%% -%% @code{Coefficients}: Contains a vector of coefficients for p(x). It follows -%% the format used for function ppval @code{opt.Coefficients(i) = a(P+1-i)}. -%% -%% @code{Damping}: Contains a vector of the coefficients for g(x'). Same format -%% as before. -%% -%% @code{Actuation}: An optional field of the structure. If it is present, it -%% defines the function f(t,q,q'). It can be a handle to a function of the form f = -%% func(@var{t}, @var{x}, @var{opt}) or it can be a @code{1xnT} vector. -%% -%% @strong{OUTPUT} -%% -%% @var{dotx}: Derivative of the state space vector with respect to time. A @code{2xnT} array. -%% -%% @var{dotxdx}: When requested, it contains the Jacobian of the system. It is a -%% multidimensional array of size @code{2x2xnT}. -%% -%% @var{u}: If present, the function returns the inputs that generate the -%% sequence of state space vectors provided in @var{x}. To do this the functions -%% estimates the second derivative of q using spline interpolation. This implies -%% that there be at least 2 observations of the state vector @var{x}, i.e. @code{nT -%% >= 2}. Otherwise the output is empty. -%% -%% @seealso{setnloscillator, ppval, odepkg} -%% @end deftypefn - -function [dotx dotxdx f] = nloscillator (t, x, opt) - - coef = -[opt.Coefficients 0]; - - damp = -[opt.Damping 0]; - - dotx = zeros (size (x)); - - dotx(1,:) = x(2,:); - ac = polyval (coef, x(1,:)) + polyval (damp, x(2,:)); - - F= zeros (1, size (x,2)); - - if isfield (opt, 'Actuation') - F = opt.Actuation; - if ~isnumeric (F); - F = F (t, x, opt); - end - end - - dotx(2,:) = ac + F; - - if nargout > 1 - nt = size (x, 2); - dotxdx = zeros (2, 2, nt); - dcoef = polyder (coef); - ddamp = polyder (damp); - - dotxdx(1,:,:) = [ zeros(1,nt); ones(1,nt) ]; - dotxdx(2,:,:) = [ polyval(dcoef, x(1,:)); polyval(ddamp, x(2,:)) ]; - end - - f = []; - if size (x,2) >= 2 && nargout > 2 - %% inverse dynamics - aci = ppval (ppder (spline (t, x(2,:))), t); - f = aci' - ac; - end - -end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/MSNForces.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,205 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {[@var{x}, @var{M}, @var{S}, @var{N}] =} MSNForces(@var{joints}, @var{members}, @var{dist}, @var{point}, @var{MemF}, @var{membernum}, @var{divisions}) +## +## This function returns the internal forces of a member for each position x. The member +## is divided in 20 subelements if the argument is not given. The used sign convention is displayed in the help file. +## +## Input parameters are similar as with SolveFrame and PlotFrame with extra arguments: +## membernum = Number of the member to calculate +## divisions = Number of divisions for the member +## +## @end deftypefn + +function [x,M,S,N]=MSNForces(joints,members,dist,point,MemF,membernum,divisions) + if (nargin < 6) + usage("[x,M,S,N]=MSNForces(joints,members,dist,point,MemF,membernum,divisions) use the help command for more information"); + end + if (nargin<7) + divisions=20; + end + Near=members(membernum,1); + Far=members(membernum,2); + L=sqrt((joints(Near,1)-joints(Far,1))^2+(joints(Near,2)-joints(Far,2))^2); + x=0:L/divisions:L; + M=zeros(1,columns(x)); + S=zeros(1,columns(x)); + N=zeros(1,columns(x)); + FxN_end=MemF(membernum,1); + FyN_end=MemF(membernum,2); + MzN_end=MemF(membernum,3); + + %Moment forces + for i=1:columns(x) + %due to end forces + M(i)+=-MzN_end+FyN_end*x(i); + %due to dist + for j=1:rows(dist) + if (dist(j,1)==membernum) + FxN=dist(j,2); + FyN=dist(j,3); + FxF=dist(j,4); + FyF=dist(j,5); + a=dist(j,6); + b=dist(j,7); + local=dist(j,8); + if (local==0) + %convert FxN,... to local axes + temp=[FxN,FyN,0,FxF,FyF,0]; + temp=(TransformationMatrix(joints(Near,1),joints(Near,2),joints(Far,1),joints(Far,2))')^-1*temp'; + FxN=temp(1); + FyN=temp(2); + FxF=temp(4); + FyF=temp(5); + end + if (x(i)>L-b) + M(i)+= (FyN+FyF)/2*(L-a-b)* (x(i)-((FyN+2*FyF)*L+(2*a-b)*FyN+(a-2*b)*FyF)/(3*FyN+3*FyF)); + end + if (x(i)>a && x(i)<=L-b) + %% bug :S zie bug.m + %M(i)+=(((3*FyN*x(i)+3*a*FyN)*L+(2*FyF-2*FyN)*x(i)^2+((-3*b-2*a)*FyN-a*FyF)*x(i)+(-3*a*b-2*a^2)*FyN-a^2*FyF)*((2*FyN*x(i)*L+(FyF-FyN)*x(i)^2+(-2*b*FyN-2*a*FyF)*x(i))/(2*L-2*b-2*a)-(2*a*FyN*L+(-2*a*b-a^2)*FyN-a^2*FyF)/(2*L-2*b-2*a)))/(6*FyN*L+(3*FyF-3*FyN)*x(i)+(-6*b-3*a)*FyN-3*a*FyF); + M(i)+=((3*FyN*x(i)^2-6*a*FyN*x(i)+3*a^2*FyN)*L+(FyF-FyN)*x(i)^3+(-3*b*FyN-3*a*FyF)*x(i)^2+((6*a*b+3*a^2)*FyN+3*a^2*FyF)*x(i)+(-3*a^2*b-2*a^3)*FyN-a^3*FyF)/(6*L-6*b-6*a); + end + end + end + %due to point load on member + for j=1:rows(point) + if (point(j,1)==membernum) + Fx=point(j,2); + Fy=point(j,3); + a=point(j,4); + local=point(j,5); + if (local==0) + %convert to global axes + temp=[Fx,Fy,0,0,0,0]; + temp=(TransformationMatrix(joints(Near,1),joints(Near,2),joints(Far,1),joints(Far,2))')^-1*temp'; + Fx=temp(1); + Fy=temp(2); + end + if (x(i)>a) + M(i)+=(x(i)-a)*Fy; + end + end + end + + end + M=M.*-1; %sign convention changed during development + + %Shear forces + for i=1:columns(x) + %due to end forces + S(i)+=FyN_end; + %due to dist + for j=1:rows(dist) + if (dist(j,1)==membernum) + FxN=dist(j,2); + FyN=dist(j,3); + FxF=dist(j,4); + FyF=dist(j,5); + a=dist(j,6); + b=dist(j,7); + local=dist(j,8); + if (local==0) + %convert FxN,... to local axes + temp=[FxN,FyN,0,FxF,FyF,0]; + temp=(TransformationMatrix(joints(Near,1),joints(Near,2),joints(Far,1),joints(Far,2))')^-1*temp'; + FxN=temp(1); + FyN=temp(2); + FxF=temp(4); + FyF=temp(5); + end + if (x(i)>L-b) + S(i)+=((FyN+FyF)*L^2+((-2*b-2*a)*FyF-2*b*FyN)*L+b^2*FyN+(b^2+2*a*b)*FyF)/(2*L-2*b-2*a)-(2*a*FyN*L+(-2*a*b-a^2)*FyN-a^2*FyF)/(2*L-2*b-2*a); + end + if (x(i)>a && x(i)<=L-b) + S(i)+=(2*FyN*x(i)*L+(FyF-FyN)*x(i)^2+(-2*b*FyN-2*a*FyF)*x(i))/(2*L-2*b-2*a)-(2*a*FyN*L+(-2*a*b-a^2)*FyN-a^2*FyF)/(2*L-2*b-2*a); + end + end + end + %due to point load on member + for j=1:rows(point) + if (point(j,1)==membernum) + Fx=point(j,2); + Fy=point(j,3); + a=point(j,4); + local=point(j,5); + if (local==0) + %convert to global axes + temp=[Fx,Fy,0,0,0,0]; + temp=(TransformationMatrix(joints(Near,1),joints(Near,2),joints(Far,1),joints(Far,2))')^-1*temp'; + Fx=temp(1); + Fy=temp(2); + end + if (x(i)>a) + S(i)+=Fy; + end + end + end + end + %Normal forces + for i=1:columns(x) + %due to end forces + N(i)+=-FxN_end; + %due to dist + for j=1:rows(dist) + if (dist(j,1)==membernum) + FxN=dist(j,2); + FyN=dist(j,3); + FxF=dist(j,4); + FyF=dist(j,5); + a=dist(j,6); + b=dist(j,7); + local=dist(j,8); + if (local==0) + %convert FxN,... to local axes + temp=[FxN,FyN,0,FxF,FyF,0]; + temp=(TransformationMatrix(joints(Near,1),joints(Near,2),joints(Far,1),joints(Far,2))')^-1*temp'; + FxN=temp(1); + FyN=temp(2); + FxF=temp(4); + FyF=temp(5); + end + if (x(i)>L-b) + N(i)-=((FxN+FxF)*L^2+((-2*b-2*a)*FxF-2*b*FxN)*L+b^2*FxN+(b^2+2*a*b)*FxF)/(2*L-2*b-2*a)-(2*a*FxN*L+(-2*a*b-a^2)*FxN-a^2*FxF)/(2*L-2*b-2*a); + end + if (x(i)>a && x(i)<=L-b) + N(i)-=(2*FxN*x(i)*L+(FxF-FxN)*x(i)^2+(-2*b*FxN-2*a*FxF)*x(i))/(2*L-2*b-2*a)-(2*a*FxN*L+(-2*a*b-a^2)*FxN-a^2*FxF)/(2*L-2*b-2*a); + end + end + end + %due to point load on member + for j=1:rows(point) + if (point(j,1)==membernum) + Fx=point(j,2); + Fy=point(j,3); + a=point(j,4); + local=point(j,5); + if (local==0) + %convert to global axes + temp=[Fx,Fy,0,0,0,0]; + temp=(TransformationMatrix(joints(Near,1),joints(Near,2),joints(Far,1),joints(Far,2))')^-1*temp'; + Fx=temp(1); + Fy=temp(2); + end + if (x(i)>a) + N(i)-=Fx; + end + end + end + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/PlotDiagrams.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,198 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {} PlotDiagrams(@var{joints}, @var{members}, @var{dist}, @var{point}, @var{MemF}, @var{diagram}, @var{divisions}, @var{scale}) +## +## This function plots the internal forces for all members. The force to be plotted can be selected with @var{diagram} +## which will be "M", "S" or "N" for the moment, shear or normal forces. +## +## Input parameters are similar as with SolveFrame and PlotFrame. +## @end deftypefn + +function PlotDiagrams(joints,members,dist,point,MemF,diagram,divisions,scale) + %during development some errors occured on windows: http://wiki.octave.org/wiki.pl?OctaveForWindows see oct2mat and plot package + + %% scaling goes wrong when there al members have end moment 0 + if (nargin < 6) + usage("PlotDiagrams(joints,members,dist,point,MemF,diagram,scale) use the help command for more information"); + end + if (nargin<7) + divisions=20; + end + if (diagram=="M") + if (nargin<8) + scale=2/max(max(abs(MemF(:,[3,6])))) + end + %plot frame first + newplot(); + clf(); + hold off; + set (gca(), "dataaspectratio", [1,1,1]); + hold on; + + for i=1:rows(members) + Near=members(i,1); %near joint + Far=members(i,2); %far joint + %make line from near to far joint + line([joints(Near,1),joints(Far,1)],[joints(Near,2),joints(Far,2)],"color","blue"); + + end + %plot coord numbers + for i=1:rows(joints) + text(joints(i,1),joints(i,2),int2str(i)); %'color','blue' + hold on; + end + %plot member numbers + for i=1:rows(members) + text((joints(members(i,1),1)+joints(members(i,2),1))/2,(joints(members(i,1),2)+joints(members(i,2),2))/2,int2str(i),'color','blue'); + end + + for i=1:rows(members) + [x,M]=MSNForces(joints,members,dist,point,MemF,i,divisions); + %finding maximum and minimum + [M_max,x_max]=max(M); + [M_min,x_min]=min(M); + %scale example scale factor: 2/max moment + M=M.*scale; % moment curve consistent with deformation + Near=members(i,1); + Far=members(i,2); + + %transformation to real member position! + L=sqrt((joints(Near,1)-joints(Far,1))^2+(joints(Near,2)-joints(Far,2))^2); + l=(joints(Far,1)-joints(Near,1))/L; + m=(joints(Far,2)-joints(Near,2))/L; + T=[l,m;-m,l]'; + for i=1:columns(x) + temp=T*[x(i);M(i)]; %rotation + x(i)=temp(1)+joints(Near,1); %displacement x and y + M(i)=temp(2)+joints(Near,2); + end + plot(x,M,'r-'); + %plot max and min + text(x(x_max),M(x_max),num2str(M_max,'%g'),'color','red'); + text(x(x_min),M(x_min),num2str(M_min,'%g'),'color','red'); + end + end + if (diagram=="S") + if (nargin<8) + scale=2/max(max(abs(MemF(:,[2,5])))); + end + %plot frame first + newplot(); + clf(); + hold off; + set (gca(), "dataaspectratio", [1,1,1]); + hold on; + + for i=1:rows(members) + Near=members(i,1); %near joint + Far=members(i,2); %far joint + %make line from near to far joint + line([joints(Near,1),joints(Far,1)],[joints(Near,2),joints(Far,2)],"color","blue"); + + end + %plot coord numbers + for i=1:rows(joints) + text(joints(i,1),joints(i,2),int2str(i)); %'color','blue' + hold on; + end + %plot member numbers + for i=1:rows(members) + text((joints(members(i,1),1)+joints(members(i,2),1))/2,(joints(members(i,1),2)+joints(members(i,2),2))/2,int2str(i),'color','blue'); + end + + for i=1:rows(members) + [x,M,S]=MSNForces(joints,members,dist,point,MemF,i,divisions); + %finding maximum and minimum + [S_max,x_max]=max(S); + [S_min,x_min]=min(S); + %scale example scale factor: 2/max moment + S=S.*scale; + Near=members(i,1); + Far=members(i,2); + + %transformation to real member position! + L=sqrt((joints(Near,1)-joints(Far,1))^2+(joints(Near,2)-joints(Far,2))^2); + l=(joints(Far,1)-joints(Near,1))/L; + m=(joints(Far,2)-joints(Near,2))/L; + T=[l,m;-m,l]'; + for i=1:columns(x) + temp=T*[x(i);S(i)]; + x(i)=temp(1)+joints(Near,1); + S(i)=temp(2)+joints(Near,2); + end + plot(x,S,'r-'); + %plot max and min + text(x(x_max),S(x_max),num2str(S_max,'%g'),'color','red'); + text(x(x_min),S(x_min),num2str(S_min,'%g'),'color','red'); + end + end + if (diagram=="N") + if (nargin<8) + scale=2/max(max(abs(MemF(:,[1,4])))); + end + %plot frame first + newplot(); + clf(); + hold off; + set (gca(), "dataaspectratio", [1,1,1]); + hold on; + + for i=1:rows(members) + Near=members(i,1); %near joint + Far=members(i,2); %far joint + %make line from near to far joint + line([joints(Near,1),joints(Far,1)],[joints(Near,2),joints(Far,2)],"color","blue"); + + end + %plot coord numbers + for i=1:rows(joints) + text(joints(i,1),joints(i,2),int2str(i)); %'color','blue' + hold on; + end + %plot member numbers + for i=1:rows(members) + text((joints(members(i,1),1)+joints(members(i,2),1))/2,(joints(members(i,1),2)+joints(members(i,2),2))/2,int2str(i),'color','blue'); + end + + for i=1:rows(members) + [x,M,S,N]=MSNForces(joints,members,dist,point,MemF,i,divisions); + %finding maximum and minimum + [N_max,x_max]=max(N); + [N_min,x_min]=min(N); + %scale example scale factor: 2/max moment + N=N.*scale; + Near=members(i,1); + Far=members(i,2); + + %transformation to real member position! + L=sqrt((joints(Near,1)-joints(Far,1))^2+(joints(Near,2)-joints(Far,2))^2); + l=(joints(Far,1)-joints(Near,1))/L; + m=(joints(Far,2)-joints(Near,2))/L; + T=[l,m;-m,l]'; + for i=1:columns(x) + temp=T*[x(i);N(i)]; + x(i)=temp(1)+joints(Near,1); + N(i)=temp(2)+joints(Near,2); + end + plot(x,N,'r-'); + %plot max and min + text(x(x_max),N(x_max),num2str(N_max,'%g'),'color','red'); + text(x(x_min),N(x_min),num2str(N_min,'%g'),'color','red'); + end + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/PlotFrame.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,78 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {} PlotFrame(@var{joints}, @var{members}, @var{D}, @var{factor}) +## +## +## Plots a 2D frame (with displacements if needed) using +## the following input parameters: +## +## joints = [x , y, constraints ; ...] +## +## constraints=[x , y, rotation] free=0, supported=1 +## +## members = [nodeN, nodeF, E, I, A; ...] +## +## Optional arguments: +## +## D = [x,y,rotation;...] Displacements as returned by SolveFrame +## +## factor= Scaling factor for the discplacements (default: 10) +## +## @end deftypefn + +function PlotFrame(joints,members,D,factor) + if (nargin < 2) + usage("PlotFrame(joints,members,D,factor) D and factor ar optional. Use the help command for more information"); + end + %during development some errors occured on windows: http://wiki.octave.org/wiki.pl?OctaveForWindows see oct2mat and plot package + newplot(); + clf(); + set (gca(), "dataaspectratio", [1,1,1]); + for i=1:rows(members) + N=members(i,1); %near joint + F=members(i,2); %far joint + %make line from near to far joint + line([joints(N,1),joints(F,1)],[joints(N,2),joints(F,2)],"color","blue"); + hold on; + if (nargin>=3) + %plot also displacements + if (nargin==3) + % default factor + factor=10; + end + cN=node2code(N); + cF=node2code(F); + line([joints(N,1)+D(N,1)*factor,joints(F,1)+D(F,1)*factor],[joints(N,2)+D(N,2)*factor,joints(F,2)+D(F,2)*factor],"color","red"); + hold on; + end + end + %one meter extra as a border + set (gca (), "xlim", [min(joints(:,1))-1,max(joints(:,1))+1]); + set (gca (), "ylim", [min(joints(:,2))-1,max(joints(:,2))+1]); + + %plot coord numbers + for i=1:rows(joints) + text(joints(i,1),joints(i,2),int2str(i)); %'color','blue' + hold on; + end + %plot member numbers + for i=1:rows(members) + text((joints(members(i,1),1)+joints(members(i,2),1))/2,(joints(members(i,1),2)+joints(members(i,2),2))/2,int2str(i),'color','blue'); + end + hold off; +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/SolveFrame.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,184 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {[@var{Reactions}, @var{Displacements}, @var{MemF}] = } SolveFrame(@var{joints}, @var{members}, @var{nodeloads}, @var{dist}, @var{point}) +## +## +## Solves a 2D frame with the matrix displacement method for +## the following input parameters: +## +## joints = [x , y, constraints ; ...] +## +## constraints=[x , y, rotation] free=0, supported=1 +## +## members = [nodeN, nodeF, E, I, A; ...] +## +## nodeloads = [node, Fx, Fy, Mz; ...] +## +## loads on members: +## +## dist = [membernum,FxN,FyN,FxF,FyF,a,b,local ; ...] for distributed loads +## where FxN and FyN are the loads on distance a from the near node +## (same with far node and distance b) +## local=1 if loads are on local axis, 0 if global +## +## point = [membernum,Fx,Fy,a,local; ...] +## where Fx and Fy are the loads on distance a from the node near +## local=1 if loads are on local axis, 0 if global +## +## Output is formated as follows (rownumber corresponds to +## node or member number): +## +## Reactions = [Fx,Fy,Mz;...] where NaN if it was a non supported dof +## +## Displacements = [x,y,rotation;...] +## +## MemF = [FxN, FyN, MzN, FxF, FyF, MzF; ...] +## @end deftypefn + +function [Reactions,Displacements,MemF]=SolveFrameOpt(joints,members,nodeloads,dist,point) + if (nargin < 5) + usage("[Reactions,Displacements,MemF]=SolveFrame(joints,members,nodeloads,dist,point) Use the help command for more information"); + end + % calc info: + % Elements Axis + % y| + % | + % |___________________________ x + % Near far + % joints: [x, y, constraints; ...] 1= fixed + % members [nodeN,nodeF,E,I,A; ...] + % nodeloads [node,Fx,Fy,Mz; ...] + + P=D=zeros(rows(joints)*3,1); + %add nodal loads to P matrix + for load=1:rows(nodeloads) + c=node2code(nodeloads(load,1)); + for i=0:2 + P(c(1+i))=P(c(1+i))+nodeloads(load,2+i); + end + end + free=[]; %contains unconstrainted codenumbers + supported=[]; %contains constrainted codenumbers + for node=1:rows(joints) + c=node2code(node); + for i=3:5 + if (joints(node,i)==0) + free=[free,c(i-2)]; + else + supported=[supported,c(i-2)]; + end + end + end + + %% global equation + %% { P_f } [ K_ff K_fs ] { Delta_f } { P_F_f } + %% { } = [ ] . { } + { } + %% { P_s } [ K_sf K_ss ] { Delta_s } { P_F_s } + %% Solutions: + %% Delta_f = K_ff^-1 (P_f - P_F_f) + %% P_s = K_sf * Delta_f + P_F_s + + %calculate transformation matrix and stiffnesmatrix for all members + k_array=MemberStiffnessMatrices(joints,members); + T_array=MemberTransformationMatrices(joints,members); + + %stiffness matrix + [ K_ff, K_sf ] = GlobalStiffnessMatrixRegrouped (joints, members,free,supported,k_array,T_array); %K_ss, K_fs are not used and if not calculated script is faster + + + %nodal forces and equivalent nodal ones + [P_F,MemFEF]=EquivalentJointLoads(joints,members,dist,point); + + + %reduced matrices + P_f=P(free,:); + P_s=P(supported,:); + + P_F_f=P_F(free,:); + P_F_s=P_F(supported,:); + + %solution: find unknown displacements + + try + %A X = B => solve with cholesky => X = R\(R'\B) + r = chol (K_ff); + D_f=r\(r'\(P_f-P_F_f)); + %D_f=cholinv(K_ff)*(P_f-P_F_f); %TODO: use this line but for testing purposes same method as old on + catch + error("System is unstable because K_ff is singular. Please check the support constraints!"); + end_try_catch + + %TODO: make use of iterative solver as an option. How???? (old code below endfunction) + + D(free,1)=D_f; + + %solution: find unknown (reaction) forces + P_s=K_sf*D_f+P_F_s; + P(supported,1)=P_s; + + + %solution: find unknown member forces + MemF=[]; + for member=1:rows(members) + N=members(member,1); + F=members(member,2); + xN=joints(N,1); + yN=joints(N,2); + xF=joints(F,1); + yF=joints(F,2); + %T=TransformationMatrix(xN,yN,xF,yF); + %k=MemberStiffnessMatrix(sqrt((xN-xF)**2+(yN-yF)**2),members(member,3),members(member,4),members(member,5)); + c=[node2code(N),node2code(F)]; + MemF=[MemF;(k_array{member,1}*T_array{member,1}*D(c'))']; + end + MemF=MemF+MemFEF;%+fixed end forces + + %format codenumbers to real output + %TODO: not efficient enough due to A=[A;newrow] + Displacements=[]; + Reactions=[]; + for i=0:rows(joints)-1 + Displacements=[Displacements;D(1+3*i:3+3*i)']; + Reactions=[Reactions;P(1+3*i:3+3*i)']; + end + for i=1:rows(Reactions) + for j=1:columns(Reactions) + if (joints(i,j+2)==0) + Reactions(i,j)=NaN; + end + end + end +end + + +%[D_f, flag] = pcg(K_ff,P_f-P_F_f,1e-6,1000); +%if (flag==1) + %max iteration +% printf('max iteration'); +% try +% %A X = B => solve with cholesky => X = R\(R'\B) +% r = chol (K_ff); +% D_f=r\(r'\(P_f-P_F_f)); +% catch +% error("System is unstable because K_ff is singular. Please check the support constraints!"); +% end_try_catch +%end +%if (flag==3) + %K_ff was found not positive definite +% error("System is unstable because K_ff is singular. Please check the support constraints!"); +%end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/SolveFrameCases.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,158 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +## -*- texinfo -*- +## @deftypefn {Function File} {[@var{results}] =} SolveFrameCases (@var{joints}, @var{members}, @var{loadcases}) +## +## +## Solves a 2D frame with the matrix displacement method for +## the following input parameters: +## +## joints = [x , y, constraints ; ...] +## +## constraints=[x , y, rotation] free=0, supported=1 +## +## members = [nodeN, nodeF, E, I, A; ...] +## +## loadcases is a struct array with for each loadcase the fields +## +## - nodeloads = [node, Fx, Fy, Mz; ...] +## +## - dist = [membernum,FxN,FyN,FxF,FyF,a,b,local ; ...] +## +## - point = [membernum,Fx,Fy,a,local; ...] +## +## input is as for the function SolveFrame. +## +## Output is a struct array with the fields: Displacements, Reactions and MemF +## +## (output formated as for the function SolveFrame.) +## @end deftypefn + +function [results]=SolveFrameCases(joints,members,loadcases) + if (nargin != 3) + usage("[Reactions,Displacements,MemF]=SolveFrame(joints,members,loadcases) Use the help command for more information"); + end + + free=[]; %contains unconstrainted codenumbers + supported=[]; %contains constrainted codenumbers + for node=1:rows(joints) + c=node2code(node); + for i=3:5 + if (joints(node,i)==0) + free=[free,c(i-2)]; + else + supported=[supported,c(i-2)]; + end + end + end + + %stiffness matrix + [ K_ff, K_sf ] = GlobalStiffnessMatrixRegrouped (joints, members,free,supported); %K_ss, K_fs are not used and if not calculated script is faster + + %from now nodeloads,dist,point must depend on case: implement with ? !!!!!!!! + for lc=1:length(loadcases) + + %convert loadcases to other variables + nodeloads=loadcases(lc).nodeloads; + dist=loadcases(lc).dist; + point=loadcases(lc).point; + + P=D=zeros(rows(joints)*3,1); + %add nodal loads to P matrix + for load=1:rows(nodeloads) + c=node2code(nodeloads(load,1)); + for i=0:2 + P(c(1+i))=P(c(1+i))+nodeloads(load,2+i); + end + end + + %nodal forces and equivalent nodal ones + [P_F,MemFEF]=EquivalentJointLoads(joints,members,dist,point); + + + %reduced matrices + P_f=P(free,:); + P_s=P(supported,:); + + P_F_f=P_F(free,:); + P_F_s=P_F(supported,:); + + + %solution: find unknown displacements + %can be quicker for large systems with sparse matrix ! + + [D_f, flag] = pcg(K_ff,P_f-P_F_f,1e-6,1000); + if (flag==1) + %max iteration + printf('max iteration'); + try + %A X = B => solve with cholesky => X = R\(R'\B) + r = chol (K_ff); + D_f=r\(r'\(P_f-P_F_f)); + catch + error("System is unstable because K_ff is singular. Please check the support constraints!"); + end_try_catch + end + if (flag==3) + %K_ff was found not positive defnite + error("System is unstable because K_ff is singular. Please check the support constraints!"); + end + + + + D(free,1)=D_f; + + %solution: find unknown (reaction) forces + P_s=K_sf*D_f+P_F_s; + P(supported,1)=P_s; + + + %find unknown member forces + MemF=[]; + for member=1:rows(members) + N=members(member,1); + F=members(member,2); + xN=joints(N,1); + yN=joints(N,2); + xF=joints(F,1); + yF=joints(F,2); + T=TransformationMatrix(xN,yN,xF,yF); + k=MemberStiffnessMatrix(sqrt((xN-xF)**2+(yN-yF)**2),members(member,3),members(member,4),members(member,5)); + c=[node2code(N),node2code(F)]; + MemF=[MemF;(k*T*D(c'))']; + end + MemF=MemF+MemFEF;%+fixed end forces + + %format codenumbers to real output + Displacements=[]; + Reactions=[]; + for i=0:rows(joints)-1 + Displacements=[Displacements;D(1+3*i:3+3*i)']; + Reactions=[Reactions;P(1+3*i:3+3*i)']; + end + for i=1:rows(Reactions) + for j=1:columns(Reactions) + if (joints(i,j+2)==0) + Reactions(i,j)=NaN; + end + end + end + results(lc).Displacements=Displacements; + results(lc).Reactions=Reactions; + results(lc).MemF=MemF; + end +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/ocframe_ex1.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,24 @@ +## -*- texinfo -*- +## @deftypefn {Function File} {} ocframe_ex1() +## Example of a planar frame. +## +## @end deftypefn +function [P,D,MemF]=ocframe_ex1() + joints=[0,0,1,1,1; + 4,4,0,0,0; + 8,4,1,1,0]; + + EIA=[210e9,23130*(10^-2)^4,84.5*(10^-2)^2];%IPE400 + + members=[1,2,EIA;2,3,EIA]; + + nodeloads=[]; + + dist=[1,0,-2e3,0,-2e3,0,0,0;2,0,-4e3,0,-4e3,0,0,1]; + point=[];%1,0,-3e4,3,1 + + [P,D,MemF]=SolveFrame(joints,members,nodeloads,dist,point); + %PlotFrame(joints,members,D,10); + %plot moment diagram + PlotDiagrams(joints,members,dist,point,MemF,"S"); +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/ocframe_ex2.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,36 @@ +## -*- texinfo -*- +## @deftypefn {Function File} {} ocframe_ex2() +## Example of a beam. +## +## @end deftypefn +function [P,D,MemF]=ocframe_ex2() + joints=[0,0,1,1,0; + 4,0,0,1,0; + 8,0,0,1,0]; + %Each 4 meter a node => 0,0 and 4,0 and 8,0 are the coordinates. + %The first node is a hinge and thus supported in the x and y direction => 1,1,0 for the constraints + %The following nodes are just rollers and thus supported in the y direction => 0,1,0 for the constraints + + EIA=[200e6,200e6*(10^-3)^4,6000*(10^-3)^2]; + %EIA as a single vector to be used afterwards + + members=[1,2,EIA;2,3,EIA]; + %2 members, connection node 1 to node 2 and node 2 to node 3 + + nodeloads=[]; + %there aren't nodal nodes in this example + + dist=[1,0,-4e3,0,-4e3,0,0,1; + 2,0,-4e3,0,-4e3,0,0,1]; + %both members have a distributed load which takes the full length of the member. Notice the - sign caused + %by the axes conventions + %as we are working with newtons and meters the load is -4e3 N and not -4 kN + + + [P,D,MemF]=SolveFrame(joints,members,nodeloads,dist,[]) + %solve the frame with + % P: reactions + % D: displacements + % MemF: the member end forces + +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/ocframe_ex3.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,47 @@ +## -*- texinfo -*- +## @deftypefn {Function File} {} ocframe_ex3() +## Example of a planar frame. +## +## @end deftypefn +function [P,D,MemF]=ocframe_ex3() + UNP120=[210e9,363.990e-8,17.000e-4]; + RHS=[210e9,28.900e-8,8.730e-4]; + joints=[0,0,1,1,0; + 1.25,0,0,0,0; + 1.575,0,0,0,0; + 2.5,0,0,0,0; + 3.425,0,0,0,0; + 3.75,0,0,0,0; + 5.,0,0,1,0; + 0,1.5,0,0,0; + 1.25,1.5,0,0,0; + 2.5,1.5,0,0,0; + 3.75,1.5,0,0,0; + 5,1.5,0,0,0]; + members=[1,2,UNP120; + 2,3,UNP120; + 3,4,UNP120; + 4,5,UNP120; + 5,6,UNP120; + 6,7,UNP120; + 8,9,UNP120; + 9,10,UNP120; + 10,11,UNP120; + 11,12,UNP120; + 1,8,RHS; + 2,9,RHS; + 4,10,RHS; + 6,11,RHS; + 7,12,RHS]; + + nodeloads=[3,0,-30e3,0; + 5,0,-30e3,0; + 12,30e3,0,0]; + + if (nargout>0) + [P,D,MemF]=SolveFrame(joints,members,nodeloads,[],[]); + else + [P,D,MemF]=SolveFrame(joints,members,nodeloads,[],[]) + PlotFrame(joints,members,D,1); + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/ocframe_exLC.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,68 @@ +## -*- texinfo -*- +## @deftypefn {Function File} {} ocframe_exLC() +## Example of a beam with generation of eurocode ULS +## load cases +## +## @end deftypefn + +function ocframe_exLC() + + joints=[0,0,1,1,0; + 4,0,0,1,0; + 8,0,0,1,0; + 12,0,0,1,0; + 16,0,0,1,0]; + + EIA=[200e6,200e6*(10^-3)^4,6000*(10^-3)^2]; + members=[1,2,EIA; + 2,3,EIA; + 3,4,EIA; + 4,5,EIA]; + + %there is a variable load 4kN/m to be combined with 2kN/m permanent + %eurocode ULS + %Permanent actions: if favourable 1 if not 1,35 + %Variable actions: if favourable 0 if not 1,5 + %Category A: domestic phi0 = 0,7 phi1 = 0,5 phi2 = 0,3 + % each case consists of permanent load * 1,35 plus one of the variable cases + % which makes 16 cases (try for each variable load 0 and 1,5 as the factor) + loadcases={}; + for i=0:15 + lc=toascii(dec2bin(i,4)).-48; + %no point loads and nodal loads + loadcases(i+1).nodeloads=[]; + loadcases(i+1).point=[]; + %dist load depends on case + dist=[]; + for j=1:4 + %add permanent + dist=[dist;[j,0,-2e3*1.35,0,-2e3*1.35,0,0,1]]; + if (lc(j)==1) + dist=[dist;[j,0,-4e3*0.7*1.5,0,-4e3*0.7*1.5,0,0,1]]; + end + loadcases(i+1).dist=dist; + end + end + + [results]=SolveFrameCases(joints,members,loadcases); + + %moment diagram envelope + moments=[]; + for lc=1:16 + x=[]; + M=[]; + for i=1:4 + [u,Mo]=MSNForces(joints,members,loadcases(lc).dist,loadcases(lc).point,results(lc).MemF,i,40); + x=[x,u.+(4*(i-1))]; + M=[M,Mo]; + end + moments=[moments;M]; + end + + plot(x,max(moments),x,min(moments)) + +end + + + + \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/ocframe_railwaybridge.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,46 @@ +## -*- texinfo -*- +## @deftypefn {Function File} {} ocframe_railwaybridge() +## Example taken from a real railwaybridge. +## +## @end deftypefn +function [P,D,MemF]=ocframe_railwaybridge() + joints=[0:5.3636:59;zeros(4,12)]'; + joints(1,3:4)=[1,1]; + joints(12,3:4)=[1,1]; + + temp=[]; + %parabola with height = 8 m => f(x)=(32*x)/59-(32*x^2)/3481 + for i=2:11 + temp=[temp;joints(i,1),(32*joints(i,1))/59-(32*joints(i,1)^2)/3481,0,0,0]; + end + + joints=[joints;temp]; + + %EIA + beam1=[200e9,135.7*(0.1)^4,3.204*(0.1)^2]; + beam2=[200e9,80.5*(0.1)^4,2.011*(0.1)^2]; + members=[]; + for i=1:11 + members=[members;i,i+1,beam1]; + end + members=[members;1,13,beam1]; + for i=13:21 + members=[members;i,i+1,beam1]; + end + members=[members;22,12,beam1]; + + for i=2:11 + members=[members;i,i+11,beam2]; + end + + %own weight of beams neglected + + %some forces + nodeloads=[6,0,-50e3,0;7,0,-50e3,0]; + + tic + [P,D,MemF]=SolveFrame(joints,members,nodeloads,[],[]); + toc + + %PlotFrame(joints,members,D,100); +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/EquivalentJointLoads.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,86 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function [P_F,MemFEF]=EquivalentJointLoads(joints,members,dist,point) + % joints: [x, y, constraints; ...] 1= fixed + % members [nodeN,nodeF,E,I,A; ...] + % dist containts distributed loads on members + % [membernum,FxN,FyN,FxF,FyF,a,b,local ; ...] + % point contains the point loads on members + % [membernum,Fx,Fy,a,local; ...] + % local=1 if in local coords + + P_F=zeros(rows(joints)*3,1); + MemFEF=zeros(rows(members),6); + %distributed loads + for i=1:rows(dist) + membernum=dist(i,1); + FxN=dist(i,2); + FyN=dist(i,3); + FxF=dist(i,4); + FyF=dist(i,5); + a=dist(i,6); + b=dist(i,7); + local=dist(i,8); + N=members(membernum,1); + F=members(membernum,2); + if (local==0) + %convert FxN,... to local axes + temp=[FxN,FyN,0,FxF,FyF,0]; + temp=(TransformationMatrix(joints(N,1),joints(N,2),joints(F,1),joints(F,2))')^-1*temp'; + FxN=temp(1); + FyN=temp(2); + FxF=temp(4); + FyF=temp(5); + end + [F1,F2,F3,F4,F5,F6]=FixedEndForcesDist(sqrt((joints(N,1)-joints(F,1))^2+(joints(F,2)-joints(N,2))^2),FxN,FyN,FxF,FyF,a,b); + F_local=[F1,F2,F3,F4,F5,F6]; + F_global=((TransformationMatrix(joints(N,1),joints(N,2),joints(F,1),joints(F,2))')*F_local')'; + + c=[node2code(N),node2code(F)]; + for j=1:6 + P_F(c(j))+=F_global(j); + MemFEF(membernum,j)+=F_local(j); + end + end + for i=1:rows(point) + %[membernum,Fx,Fy,a,local; ...] + membernum=point(i,1); + Fx=point(i,2); + Fy=point(i,3); + a=point(i,4); + local=point(i,5); + N=members(membernum,1); + F=members(membernum,2); + if (local==0) + %convert to global axes + temp=[Fx,Fy,0,0,0,0]; + temp=(TransformationMatrix(joints(N,1),joints(N,2),joints(F,1),joints(F,2))')^-1*temp'; + Fx=temp(1); + Fy=temp(2); + end + %FixedEndForcesPnt(l,a,Fx,Fy) + [F1,F2,F3,F4,F5,F6]=FixedEndForcesPnt(sqrt((joints(N,1)-joints(F,1))^2+(joints(F,2)-joints(N,2))^2),a,Fx,Fy); + F_local=[F1,F2,F3,F4,F5,F6]; + F_global=((TransformationMatrix(joints(N,1),joints(N,2),joints(F,1),joints(F,2))')*F_local')'; + + c=[node2code(N),node2code(F)]; + for j=1:6 + P_F(c(j))+=F_global(j); + MemFEF(membernum,j)+=F_local(j); + end + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/FixedEndForcesDist.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,39 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function [F1,F2,F3,F4,F5,F6]=FixedEndForcesDist(l,FxN,FyN,FxF,FyF,a,b) + % distributed load in local coords. + % a: distance from Near + % b: distance from Far + % FxN, FyN forces on start of load in local coordinates + % FxF, FyF forces on end of load in local coordinates + + % formulas found by integrating the formulas for a point load with maxima maxima.sourceforge.net + F1=-((2*FxN+FxF)*l^2+((-b-4*a)*FxN+(b-2*a)*FxF)*l+(-b^2+a*b+2*a^2)*FxN+(-2*b^2-a*b+a^2)*FxF)/(6*l); + F2=-1*(((7*FyN+3*FyF)*l^4+((-3*b-13*a)*FyN+(3*b-7*a)*FyF)*l^3+((-3*b^2+4*a*b-3*a^2)*FyN+(3*b^2-4*a*b+3*a^2)*FyF)*l^2+ +((-3*b^3+a*b^2+a^2*b+17*a^3)*FyN+(-17*b^3-a*b^2-a^2*b+3*a^3)*FyF)*l+(2*b^4-2*a*b^3+2*a^2*b^2-2*a^3*b-8*a^4)*FyN+(8*b^4+2*a*b^3-2*a^2*b^2+2*a^3*b-2*a^4)* +FyF)/(20*l^3)); + F3=-1*(((3*FyN+2*FyF)*l^4+((3*a-2*b)*FyN+(2*b-3*a)*FyF)*l^3+((-2*b^2+a*b-27*a^2)*FyN+(2*b^2-a*b-3*a^2)*FyF)*l^2+ +((-2*b^3-a*b^2+4*a^2*b+33*a^3)*FyN+(-18*b^3+a*b^2-4*a^2*b+7*a^3)*FyF)*l+(3*b^4-3*a*b^3+3*a^2*b^2-3*a^3*b-12*a^4)*FyN+ +(12*b^4+3*a*b^3-3*a^2*b^2+3*a^3*b-3*a^4)*FyF)/(60*l^2)); + F4=-((FxN+2*FxF)*l^2+((a-2*b)*FxN+(-4*b-a)*FxF)*l+(b^2-a*b-2*a^2)*FxN+(2*b^2+a*b-a^2)*FxF)/(6*l); + F5=-1*(((3*FyN+7*FyF)*l^4+((3*a-7*b)*FyN+(-13*b-3*a)*FyF)*l^3+((3*b^2-4*a*b+3*a^2)*FyN+(-3*b^2+4*a*b-3*a^2)*FyF)*l^2+ +((3*b^3-a*b^2-a^2*b-17*a^3)*FyN+(17*b^3+a*b^2+a^2*b-3*a^3)*FyF)*l+(-2*b^4+2*a*b^3-2*a^2*b^2+2*a^3*b+8*a^4)*FyN+(-8*b^4-2*a*b^3+2*a^2*b^2-2*a^3*b+2*a^4)* +FyF)/(20*l^3)); + F6=-1*(-((2*FyN+3*FyF)*l^4+((2*a-3*b)*FyN+(3*b-2*a)*FyF)*l^3+((-3*b^2-a*b+2*a^2)*FyN+(-27*b^2+a*b-2*a^2)*FyF)*l^2+ +((7*b^3-4*a*b^2+a^2*b-18*a^3)*FyN+(33*b^3+4*a*b^2-a^2*b-2*a^3)*FyF)*l+(-3*b^4+3*a*b^3-3*a^2*b^2+3*a^3*b+12*a^4)*FyN+ +(-12*b^4-3*a*b^3+3*a^2*b^2-3*a^3*b+3*a^4)*FyF)/(60*l^2)); +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/FixedEndForcesPnt.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,33 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function [F1,F2,F3,F4,F5,F6]=FixedEndForcesPnt(l,a,Fx,Fy) + % point load in local coords. + % a: distance from Near + % l: length + % FxN, FyN forces on start of load in local coordinates + % FxF, FyF forces on end of load in local coordinates + + % b: distance from Far + b=l-a; + + F1=-b*Fx/l; + F2=-Fy*(b/l-a^2*b/l^3+a*b^2/l^3); + F3=-Fy*a*b^2/l^2; + F4=-a*Fx/l; + F5=-Fy*(a/l+a^2*b/l^3-a*b^2/l^3); + F6=Fy*a^2*b/l^2; +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/GlobalStiffnessMatrix.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,118 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function K=GlobalStiffnessMatrix(joints,members,memberstiffnessmatrices,membertransformationmatrices) + K=sparse(rows(joints)*3,rows(joints)*3); + + # sparse matrix is even usefull for small frames: + # example: + # Attr Name Size Bytes Class + # ==== ==== ==== ===== ===== + # K_full 36x36 10368 double + # K_sparse 36x36 2836 double + + if (nargin==2) + #joints: [x, y, constraints; ...] 1= fixed + #members [nodeN,nodeF,E,I,A; ...] + for member=1:rows(members) + N=members(member,1); + F=members(member,2); + xN=joints(N,1); + yN=joints(N,2); + xF=joints(F,1); + yF=joints(F,2); + %T=TransformationMatrix(xN,yN,xF,yF); + %k=transpose(T)*MemberStiffnessMatrix(sqrt((xN-xF)**2+(yN-yF)**2),members(member,3),members(member,4),members(member,5))*T; + c=[node2code(N),node2code(F)]; + + %TransformationMatrix stuff + L=sqrt((xN-xF)^2+(yN-yF)^2); + l=(xF-xN)/L; + m=(yF-yN)/L; + %MemberStiffnessMatrix stuff + E=members(member,3); + I=members(member,4); + A=members(member,5); + + %for i=1:rows(k) + % for j=1:columns(k) + % K(c(i),c(j))=K(c(i),c(j))+k(i,j); + % end + %end + + %fill Global Stiffness Matrix + K(c(1),c(1))+=(l^2*A*E)/L+(12*m^2*E*I)/L^3; + K(c(1),c(2))+=(l*m*A*E)/L-(12*l*m*E*I)/L^3; + K(c(1),c(3))+=-(6*m*E*I)/L^2; + K(c(1),c(4))+=-(l^2*A*E)/L-(12*m^2*E*I)/L^3; + K(c(1),c(5))+=(12*l*m*E*I)/L^3-(l*m*A*E)/L; + K(c(1),c(6))+=-(6*m*E*I)/L^2; + K(c(2),c(1))+=(l*m*A*E)/L-(12*l*m*E*I)/L^3; + K(c(2),c(2))+=(m^2*A*E)/L+(12*l^2*E*I)/L^3; + K(c(2),c(3))+=(6*l*E*I)/L^2; + K(c(2),c(4))+=(12*l*m*E*I)/L^3-(l*m*A*E)/L; + K(c(2),c(5))+=-(m^2*A*E)/L-(12*l^2*E*I)/L^3; + K(c(2),c(6))+=(6*l*E*I)/L^2; + K(c(3),c(1))+=-(6*m*E*I)/L^2; + K(c(3),c(2))+=(6*l*E*I)/L^2; + K(c(3),c(3))+=(4*E*I)/L; + K(c(3),c(4))+=(6*m*E*I)/L^2; + K(c(3),c(5))+=-(6*l*E*I)/L^2; + K(c(3),c(6))+=(2*E*I)/L; + K(c(4),c(1))+=-(l^2*A*E)/L-(12*m^2*E*I)/L^3; + K(c(4),c(2))+=(12*l*m*E*I)/L^3-(l*m*A*E)/L; + K(c(4),c(3))+=(6*m*E*I)/L^2; + K(c(4),c(4))+=(l^2*A*E)/L+(12*m^2*E*I)/L^3; + K(c(4),c(5))+=(l*m*A*E)/L-(12*l*m*E*I)/L^3; + K(c(4),c(6))+=(6*m*E*I)/L^2; + K(c(5),c(1))+=(12*l*m*E*I)/L^3-(l*m*A*E)/L; + K(c(5),c(2))+=-(m^2*A*E)/L-(12*l^2*E*I)/L^3; + K(c(5),c(3))+=-(6*l*E*I)/L^2; + K(c(5),c(4))+=(l*m*A*E)/L-(12*l*m*E*I)/L^3; + K(c(5),c(5))+=(m^2*A*E)/L+(12*l^2*E*I)/L^3; + K(c(5),c(6))+=-(6*l*E*I)/L^2; + K(c(6),c(1))+=-(6*m*E*I)/L^2; + K(c(6),c(2))+=(6*l*E*I)/L^2; + K(c(6),c(3))+=(2*E*I)/L; + K(c(6),c(4))+=(6*m*E*I)/L^2; + K(c(6),c(5))+=-(6*l*E*I)/L^2; + K(c(6),c(6))+=(4*E*I)/L; + end + end + if (nargin==4) + #joints: [x, y, constraints; ...] 1= fixed + #members [nodeN,nodeF,E,I,A; ...] + for member=1:rows(members) + N=members(member,1); + F=members(member,2); + c=[node2code(N),node2code(F)]; + + xN=joints(N,1); + yN=joints(N,2); + xF=joints(F,1); + yF=joints(F,2); + %T=TransformationMatrix(xN,yN,xF,yF); + %k=transpose(T)*MemberStiffnessMatrix(sqrt((xN-xF)**2+(yN-yF)**2),members(member,3),members(member,4),members(member,5))*T; + k=transpose(membertransformationmatrices{member,1})*memberstiffnessmatrices{member,1}*membertransformationmatrices{member,1}; + + for i=1:rows(k) + for j=1:columns(k) + K(c(i),c(j))=K(c(i),c(j))+k(i,j); + end + end + end + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/GlobalStiffnessMatrixModified.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,48 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function [ Kmod, Kcor ] = GlobalStiffnessMatrixModified(joints,members,supported) + %% \cite{Felippa2010} : "To apply support conditions without rearranging the equations we clear (set to zero) rows and columns + %% corresponding to prescribed zero displacements as well as the corresponding force components, and place + %% ones on the diagonal to maintain non-singularity. The resulting system is called the modified set of master + %% stiffness equations. " + + %% joints: [x, y, constraints; ...] 1= fixed + %% members [nodeN, nodeF, E, I, A; ...] + %% supported: vector with the supported (fixed) code numbers + + %% Kmod + Kcor = K + + %% Kcor is the correction matrix to make K + + Kmod=GlobalStiffnessMatrix(joints,members); + Kcor=sparse(rows(joints)*3,rows(joints)*3); + + %% Creat Kcor + for i=1:columns(supported) + Kcor(:,i)=Kmod(:,i); + Kcor(i,:)=Kmod(i,:); + end + %% delete rows and columns. This might be not the fastest method. + for i=1:columns(supported) + Kmod(:,i)=zeros(rows(joints)*3,1); + Kmod(i,:)=zeros(1,rows(joints)*3); + + %set 1 and -1 in Kmod and Kcor + Kmod(i,i)+=1; + Kcor(i,i)-=1; + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/GlobalStiffnessMatrixRegrouped.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,57 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function [ K_ff, K_sf, K_ss, K_fs ] = GlobalStiffnessMatrixRegrouped (joints, members,free,supported,memberstiffnessmatrices,membertransformationmatrices) + %% Returns the components of the global stiffness matrix regrouped as in the following equation: + %% { P_f } [ K_ff K_fs ] { Delta_f } + %% { } = [ ] . { } + %% { P_s } [ K_sf K_ss ] { Delta_s } + + %% joints: [x, y, constraints; ...] 1= fixed + %% members [nodeN, nodeF, E, I, A; ...] + %% free: vector with the free code numbers + %% supported: vector with the supported (fixed) code numbers + + %zeros(row,col) + + %number of free en supported joints + + %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + %TODO: for large systems this function is still slow + %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + if (nargin>4) + K=GlobalStiffnessMatrix(joints,members,memberstiffnessmatrices,membertransformationmatrices); + else + K=GlobalStiffnessMatrix(joints,members); + end + + %K_ff + K_ff=K(free,free); + + %K_sf + K_sf=K(supported,free); + + + %K_ss + if (nargout>=3) + K_ss=K(supported,supported); + end + + %K_fs + if (nargout==4) + K_fs=K(free,supported); + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/GlobalStiffnessMatrixRegrouped.m.old Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,98 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function [ K_ff, K_sf, K_ss, K_fs ] = GlobalStiffnessMatrixRegrouped (joints, members,free,supported) + %% Returns the components of the global stiffness matrix regrouped as in the following equation: + %% { P_f } [ K_ff K_fs ] { Delta_f } + %% { } = [ ] . { } + %% { P_s } [ K_sf K_ss ] { Delta_s } + + %% joints: [x, y, constraints; ...] 1= fixed + %% members [nodeN, nodeF, E, I, A; ...] + %% free: vector with the free code numbers + %% supported: vector with the supported (fixed) code numbers + + %zeros(row,col) + + %number of free en supported joints + + %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + %TODO: for large systems this function is still slow + % due to the indexof! + %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + f=columns(free); + s=columns(supported); + + K_ff=spalloc(f,f); + K_ss=spalloc(s,s); + K_sf=spalloc(s,f); + K_fs=spalloc(f,s); + for member=1:rows(members) + N=members(member,1); + F=members(member,2); + xN=joints(N,1); + yN=joints(N,2); + xF=joints(F,1); + yF=joints(F,2); + T=TransformationMatrix(xN,yN,xF,yF); + k=transpose(T)*MemberStiffnessMatrix(sqrt((xN-xF)**2+(yN-yF)**2),members(member,3),members(member,4),members(member,5))*T;%global coords. + c=[node2code(N),node2code(F)];%code numbers of the near and far nodes of the current member + + for i=1:rows(k) + for j=1:columns(k) + if (k(i,j)!=0) + c_row=c(i); + c_col=c(j); + + %K_ff + row=indexof(free,c_row); + col=indexof(free,c_col); + if (row!=-1 && col!=-1) + K_ff(row,col)=K_ff(row,col)+k(i,j); + end + + %K_sf + row=indexof(supported,c_row); + col=indexof(free,c_col); + if (row!=-1 && col!=-1) + K_sf(row,col)=K_sf(row,col)+k(i,j); + end + + + %K_ss + if (nargout>=3) + row=indexof(supported,c_row); + col=indexof(supported,c_col); + if (row!=-1 && col!=-1) + K_ss(row,col)=K_ss(row,col)+k(i,j); + end + end + + %K_fs + if (nargout==4) + row=indexof(free,c_row); + col=indexof(supported,c_col); + if (row!=-1 && col!=-1) + K_fs(row,col)=K_fs(row,col)+k(i,j); + end + end + end + end + end + + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/MemberStiffnessMatrices.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,32 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +#MemberStiffnessMatrix(L,E,I,A) +function k_array=MemberStiffnessMatrices(joints,members) + + k_array = cell(rows(members),1); # is one column better than one row? + for member=1:rows(members) + N=members(member,1); + F=members(member,2); + xN=joints(N,1); + yN=joints(N,2); + xF=joints(F,1); + yF=joints(F,2); + + # calculate the member stiffness matrix in local coordinates and add to array + k_array{member,1}=MemberStiffnessMatrix(sqrt((xN-xF)**2+(yN-yF)**2),members(member,3),members(member,4),members(member,5)); + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/MemberStiffnessMatrix.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,24 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function k=MemberStiffnessMatrix(L,E,I,A) + k=[A*E/L,0,0,-A*E/L,0,0; + 0,12*E*I/L^3,6*E*I/L^2,0,-12*E*I/L^3,6*E*I/L^2; + 0,6*E*I/L^2,4*E*I/L,0,-6*E*I/L^2,2*E*I/L; + -A*E/L,0,0,A*E/L,0,0; + 0,-12*E*I/L^3,-6*E*I/L^2,0,12*E*I/L^3,-6*E*I/L^2; + 0,6*E*I/L^2,2*E*I/L,0,-6*E*I/L^2,4*E*I/L]; +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/MemberTransformationMatrices.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,30 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function T_array=MemberTransformationMatrices(joints,members) + + T_array = cell(rows(members),1); # is one column better than one row? + + for member=1:rows(members) + N=members(member,1); + F=members(member,2); + xN=joints(N,1); + yN=joints(N,2); + xF=joints(F,1); + yF=joints(F,2); + T_array{member,1}=TransformationMatrix(xN,yN,xF,yF); + end +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/TransformationMatrix.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,29 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function T=TransformationMatrix(x1,y1,x2,y2) + #Displacement transformation matrix = T + #Force transformation = T transposed + L=sqrt((x1-x2)^2+(y1-y2)^2); + l=(x2-x1)/L; + m=(y2-y1)/L; + T=[l,m,0,0,0,0; + -m,l,0,0,0,0; + 0,0,1,0,0,0; + 0,0,0,l,m,0; + 0,0,0,-m,l,0; + 0,0,0,0,0,1]; +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/indexof.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,25 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function [index] = indexof(vector,number) + index=-1; + for i=1:columns(vector) + if (vector(i)==number) + index=i; + break + end + end +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/matrix2tex.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,33 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function matrix2tex(m) + printf("[tex]\\left(\\begin{array}{") + for c=1:columns(m) + printf("c") + end + printf("}\n") + for r=1:rows(m) + for c=1:columns(m) + if (c!=columns(m)) + printf(" %3.3e &",m(r,c)) + else + printf(" %3.3e \\\\\n",m(r,c)) + end + end + end + printf("\\end{array}\\right)[/tex]\n\n") +end
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/node2code.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,19 @@ +## Copyright (C) 2010 Johan Beke +## +## This software is free software; you can redistribute it and/or modify it +## under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 3 of the License, or (at +## your option) any later version. +## +## This software is distributed in the hope that it will be useful, but +## WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +## General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this software; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +function c=node2code(node) + c=(node-1)*3+1:(node-1)*3+3; +end \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main/mechanics/inst/ocframe/private/test_FixedEndForcesPnt.m Fri Dec 09 12:22:57 2011 +0000 @@ -0,0 +1,36 @@ +## Copyright (C) 2011 Johan +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 2 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with Octave; see the file COPYING. If not, see +## <http://www.gnu.org/licenses/>. + +## test_FixedEndForcesPnt + +## Author: Johan <johan@johan-Satellite-L30> +## Created: 2011-10-02 + +function [ F1,F2,F3,F4,F5,F6 ] = test_FixedEndForcesPnt (l,a,Fx,Fy) + joints=[0,0,1,1,1; + a,0,0,0,0; + l,0,1,1,1]; + nodeloads=[2,Fx,Fy,0]; + members=[1,2,1,1,1; + 2,3,1,1,1]; + R=SolveFrame(joints,members,nodeloads,[],[]); + F1 = R(1,1); + F2 = R(1,2); + F3 = R(1,3); + F4 = R(3,1); + F5 = R(3,2); + F6 = R(3,3); +endfunction
--- a/main/mechanics/inst/pendulum.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,103 +0,0 @@ -%% Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: you can redistribute it and/or modify -%% it under the terms of the GNU General Public License as published by -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it will be useful, -%% but WITHOUT ANY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have received a copy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {[ @var{dotx}, @var{dotxdx}, @var{u}] =} pendulum (@var{t}, @var{x}, @var{opt}) -%% Implements a general pendulum. -%% @tex -%% $$ -%% \ddot{q} + \frac{g}{l}\sin(q) + d\dot{q} = f(t,q,\dot{q}) -%% $$ -%% @end tex -%% @ifnottex -%% -%% @example -%% q'' + (g/l)*sin(q) + d*q' = f(t,q,q') -%% @end example -%% -%% @end ifnottex -%% @noindent -%% where q is the angle of the pendulum and q' its angular velocity -%% -%% This function can be used with the ODE integrators. -%% -%% @strong{INPUTS} -%% -%% @var{t}: Time. It can be a scalar or a vector of length @code{nT}. -%% -%% @var{x}: State space vector. An array of size @code{2xnT}, where @code{nT} is -%% the number of time values given. The first row corresponds to the configurations -%% of the system and the second row to its derivatives with respect to time. -%% -%% @var{opt}: An options structure. See the complementary function -%% @code{setpendulum}. The structure containing the fields: -%% -%% @code{Coefficients}: Contains the coefficients (g/l). -%% -%% @code{Damping}: Contains the coefficient d. -%% -%% @code{Actuation}: An optional field of the structure. If it is present, it -%% defines the function f(t,q,q'). It can be a handle to a function of the form f = -%% func(@var{t},@var{x},@var{opt}) or it can be a @code{1xnT} vector. -%% -%% @strong{OUTPUT} -%% -%% @var{dotx}: Derivative of the state space vector with respect to time. A @code{2xnT} array. -%% -%% @var{dotxdx}: When requested, it contains the Jacobian of the system. It is a multidimensional array of size @code{2x2xnT}. -%% -%% @var{u}: If present, the function returns the inputs that generate the -%% sequence of state space vectors provided in @var{x}. To do this the functions -%% estimates the second derivative of q using spline interpolation. This implies -%% that there be at least 2 observations of the state vector @var{x}, i.e. @code{nT -%% >= 2}. Otherwise the output is empty. -%% -%% @seealso{setpendulum, nloscillator, odepkg} -%% @end deftypefn - -function [dotx dotxdx f] = pendulum (t, x, opt) - - gl = opt.Coefficients(1); - damp = opt.Damping(1); - - dotx = zeros (size (x)); - - dotx(1,:) = x(2,:); - ac = -gl*sin (x(1,:)) - damp*x(2,:); - - F= zeros (1, size (x, 2)); - if isfield (opt, 'Actuation') - F = opt.Actuation; - if ~isnumeric (F); - F = F(t, x, opt); - end - end - - dotx(2,:) = ac + F; - - if nargout > 1 - nt = size (x, 2); - dotxdx = zeros (2, 2, nt); - - dotxdx(1,:,:) = [zeros(1, nt); ones(1, nt)]; - dotxdx(2,:,:) = [-gl*cos(x(1,:)); -damp*ones(1, nt)]; - end - - if size (x, 2) >= 2 && nargout > 2 - %% inverse dynamics - aci = ppval (ppder (spline (t, x(2,:))), t); - f = aci' - ac; - end -endfunction
--- a/main/mechanics/inst/principalaxes.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,96 +0,0 @@ -## Copyright (c) 2011 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -## -## This program is free software; you can redistribute it and/or modify -## it under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or -## (at your option) any later version. -## -## This program is distributed in the hope that it will be useful, -## but WITHOUT ANY WARRANTY; without even the implied warranty of -## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -## GNU General Public License for more details. -## -## You should have received a copy of the GNU General Public License -## along with this program; if not, see <http://www.gnu.org/licenses/>. - -## -*- texinfo -*- -## @deftypefn {Function File} {[@var{axes} @var{l} @var{moments}] =} principalaxes (@var{shape}) -## Calculates the principal axes of a shape. -## -## Returns a matrix @var{axes} where each row corresponds to one of the principal -## axes of the shape. @{l} is the second moment of area around the correspoding -## principal axis. @var{axes} is order from lower to higher @var{l}. -## -## @seealso{inertiamoment, masscenter} -## @end deftypefn - -function [PA l Jm] = principalaxes (shape) - - Jm = shapemoment (shape); - - Jsq = Jm(2)^2; - if Jsq > eps; - - TrJ = Jm(1) + Jm(3); - DetJ = Jm(1)*Jm(3) - Jsq; - - %% Eigenvalues - l = ( [TrJ; TrJ] + [1; -1]*sqrt(TrJ^2 - 4*DetJ) )/2; - - %% Eginevectors (Exchanged Jx with Jy) - PA(:,1) = (l - Jm(1)) .* (l - Jm(3)) / Jsq; - PA(:,2) = (l - Jm(1)) .* (l - Jm(3)).^2 / Jm(2)^3; - - %% Normalize - PAnorm = sqrt ( sumsq(PA,2)); - PA(1,:) = PA(1,:) ./ PAnorm(1); - PA(2,:) = PA(2,:) ./ PAnorm(2); - else - - %% Matrix already diagonal - PA(:,1) = [1 ; 0]; - PA(:,2) = [0 ; 1]; - l = [Jm(3); Jm(1)]; - - end - - %% First axis is the one with lowest moment - [l ind] = sort (l, 'ascend'); - PA = PA(ind([2 1]),:); - - %% Check that is a right hand oriented pair of axis - if PA(1,1)*PA(2,2) - PA(1,2)*PA(2,1) < 0 - PA(1,:) = -PA(1,:); - end -end - -%!test -%! h = 1; b = 2; -%! rectangle = [-b/2 -h/2; b/2 -h/2; b/2 h/2; -b/2 h/2]; -%! [PA l] = principalaxes(rectangle); -%! assert ( [0 1; 1 0], PA, 1e-6); -%! assert ([b*h^3; h*b^3]/12, l); - -%!demo -%! t = linspace(0,2*pi,64).'; -%! shape = [cos(t)-0.3*cos(3*t) sin(t)](1:end-1,:); -%! shapeR = shape*rotv([0 0 1],pi/4)(1:2,1:2); -%! [PAr l] = principalaxes(shapeR); -%! [PA l] = principalaxes(shape); -%! -%! cla -%! plot(shape(:,1),shape(:,2),'-k'); -%! line([0 PA(1,1)],[0 PA(1,2)],'color','r'); -%! line([0 PA(2,1)],[0 PA(2,2)],'color','b'); -%! -%! hold on -%! -%! plot(shapeR(:,1)+3,shapeR(:,2),'-k'); -%! line([3 PAr(1,1)+3],[0 PAr(1,2)],'color','r'); -%! line([3 PAr(2,1)+3],[0 PAr(2,2)],'color','b'); -%! -%! axis equal -%! axis square - - -
--- a/main/mechanics/inst/quat2mat.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,7 +0,0 @@ -function R = quat2mat (q) - -R = [ q(1)^2+q(2)^2-q(3)^2-q(4)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4) + q(1)*q(3)); ... - 2*(q(2)*q(3)+q(1)*q(4)) q(1)^2-q(2)^2+q(3)^2-q(4)^2 2*(q(3)*q(4) - q(1)*q(2)); ... - 2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) q(1)^2-q(2)^2-q(3)^2+q(4)^2 ]; - -endfunction
--- a/main/mechanics/inst/quatconj.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,23 +0,0 @@ -%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: youcan redistribute itand/or modify -%% it under the terms of the GNU General Public Licenseas publishedby -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it willbe useful, -%% but WITHOUTaNY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have receivedacopy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {@var{qc} = } quatconj (@var{q}) -%% Conjugate of a quaternion. -%% @end deftypefn - -function qc = quatconj(q); - qc = [q(:,1) -q(:,2:4)]; -endfunction
--- a/main/mechanics/inst/quatprod.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,29 +0,0 @@ -%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: youcan redistribute itand/or modify -%% it under the terms of the GNU General Public Licenseas publishedby -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it willbe useful, -%% but WITHOUTaNY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have receivedacopy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {@var{q3} = } quatprod (@var{q1}, @var{q2}) -%% Product of two quaternions. -%% @end deftypefn - -function q3 = quatprod (q1,q2) - -q3 = [ q1(:,1).*q2(:,1) - q1(:,2).*q2(:,2) - q1(:,3).*q2(:,3) - q1(:,4).*q2(:,4) ... - q1(:,1).*q2(:,2) + q1(:,2).*q2(:,1) + q1(:,3).*q2(:,4) - q1(:,4).*q2(:,3) ... - q1(:,1).*q2(:,3) - q1(:,2).*q2(:,4) + q1(:,3).*q2(:,1) + q1(:,4).*q2(:,2) ... - q1(:,1).*q2(:,4) + q1(:,2).*q2(:,3) - q1(:,3).*q2(:,2) + q1(:,4).*q2(:,1) ]; - -endfunction -
--- a/main/mechanics/inst/quatvrot.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,23 +0,0 @@ -%%copyright (c) 2011 Juan Pablocarbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: youcan redistribute itand/or modify -%% it under the terms of the GNU General Public Licenseas publishedby -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it willbe useful, -%% but WITHOUTaNY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FORa PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have receivedacopy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {@var{vn} = } quatvrot (@var{v}, @var{q}) -%% Rotate vector v accoding to quaternionr q. -%% @end deftypefn - -function rn = quatvrot(r,q) - rn = quatprod(quatprod(q,[zeros(size(r,1),1) r]),quatconj(q))(:,2:end); -end
--- a/main/mechanics/inst/setnloscillator.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,47 +0,0 @@ -%% Copyright (c) 2010 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: you can redistribute it and/or modify -%% it under the terms of the GNU General Public License as published by -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it will be useful, -%% but WITHOUT ANY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have received a copy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {[ @var{opts}, @var{desc}] =} setnloscilator () -%% Returns the required options structure for the function nloscillator and a -%% description of the fields in the structure. -%% -%% @seealso{nloscillator} -%% @end deftypefn - -function [opt desc] = setnloscillator(varargin) - -required_fields = {'Coefficients','Damping'}; -reqf_default = {[0.5 0 1];0.01}; -reqf_description = {['Coefficients for the spring polynomial without ' ... - 'homogeneous part. The number of elements is the degree ' ... - 'of the polynomial. the first element is the leading coefficient.'];... - ['Coefficients for the damping polynomial without ' ... - 'homogeneous part. The number of elements is the degree ' ... - 'of the polynomial. the first element is the leading coefficient.']}; - -optional_fields = {'Actuation'}; -optf_description = {['Optional field. It defines the forcing function (source)'... - 'f(t). It can be a handle to a function of the form f = func(t,x,opt)' ... - 'or it can be a 1xnT array.']}; - -opt = cell2struct(reqf_default,required_fields,1); - -if nargout > 1 - cell1 = {reqf_description{:},optf_description{:}}'; - cell2 = {required_fields{:},optional_fields{:}}; - desc = cell2struct(cell1,cell2,1); -end -endfunction
--- a/main/mechanics/inst/setpendulum.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,43 +0,0 @@ -%% Copyright (c) 2010 Juan Pablo Carbajal <carbajal@ifi.uzh.ch> -%% -%% This program is free software: you can redistribute it and/or modify -%% it under the terms of the GNU General Public License as published by -%% the Free Software Foundation, either version 3 of the License, or -%% any later version. -%% -%% This program is distributed in the hope that it will be useful, -%% but WITHOUT ANY WARRANTY; without even the implied warranty of -%% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -%% GNU General Public License for more details. -%% -%% You should have received a copy of the GNU General Public License -%% along with this program. If not, see <http://www.gnu.org/licenses/>. - -%% -*- texinfo -*- -%% @deftypefn {Function File} {[ @var{opts}, @var{desc}] =} setpendulum () -%% Returns the required options structure for the function pendulum and a -%% description of the fields in the structure. -%% -%% @seealso{pendulum} -%% @end deftypefn - -function [opt desc] = setpendulum(varargin) - -required_fields = {'Coefficients','Damping'}; -reqf_default = {1;0}; -reqf_description = {['Ration (g/l), relation between gravity and length of the pendulum'];... - ['Damping coefficient, damping is proportional to angular speed.']}; - -optional_fields = {'Actuation'}; -optf_description = {['Optional field. It defines the forcing function (source)'... - "f(t,q,q'). It can be a handle to a function of the form f = func(t,x,opt)" ... - 'or it can be a 1xnT array.']}; - -opt = cell2struct(reqf_default,required_fields,1); - -if nargout > 1 - cell1 = {reqf_description{:},optf_description{:}}'; - cell2 = {required_fields{:},optional_fields{:}}; - desc = cell2struct(cell1,cell2,1); -end -endfunction
--- a/main/mechanics/ocframe_deprecated/SolveFrame.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,181 +0,0 @@ -## Copyright (C) 2010 Johan Beke -## -## This software is free software; you can redistribute it and/or modify it -## under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or (at -## your option) any later version. -## -## This software is distributed in the hope that it will be useful, but -## WITHOUT ANY WARRANTY; without even the implied warranty of -## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -## General Public License for more details. -## -## You should have received a copy of the GNU General Public License -## along with this software; see the file COPYING. If not, see -## <http://www.gnu.org/licenses/>. - -## -*- texinfo -*- -## @deftypefn {Function File} {[@var{Reactions}, @var{Displacements}, @var{MemF}] = } SolveFrame(@var{joints}, @var{members}, @var{nodeloads}, @var{dist}, @var{point}) -## -## -## Solves a 2D frame with the matrix displacement method for -## the following input parameters: -## -## joints = [x , y, constraints ; ...] -## -## constraints=[x , y, rotation] free=0, supported=1 -## -## members = [nodeN, nodeF, E, I, A; ...] -## -## nodeloads = [node, Fx, Fy, Mz; ...] -## -## loads on members: -## -## dist = [membernum,FxN,FyN,FxF,FyF,a,b,local ; ...] for distributed loads -## where FxN and FyN are the loads on distance a from the near node -## (same with far node and distance b) -## local=1 if loads are on local axis, 0 if global -## -## point = [membernum,Fx,Fy,a,local; ...] -## where Fx and Fy are the loads on distance a from the node near -## local=1 if loads are on local axis, 0 if global -## -## Output is formated as follows (rownumber corresponds to -## node or member number): -## -## Reactions = [Fx,Fy,Mz;...] where NaN if it was a non supported dof -## -## Displacements = [x,y,rotation;...] -## -## MemF = [FxN, FyN, MzN, FxF, FyF, MzF; ...] -## @end deftypefn - -function [Reactions,Displacements,MemF]=SolveFrame(joints,members,nodeloads,dist,point) - if (nargin < 5) - usage("[Reactions,Displacements,MemF]=SolveFrame(joints,members,nodeloads,dist,point) Use the help command for more information"); - end - % calc info: - % Elements Axis - % y| - % | - % |___________________________ x - % Near far - % joints: [x, y, constraints; ...] 1= fixed - % members [nodeN,nodeF,E,I,A; ...] - % nodeloads [node,Fx,Fy,Mz; ...] - - P=D=zeros(rows(joints)*3,1); - %add nodal loads to P matrix - for load=1:rows(nodeloads) - c=node2code(nodeloads(load,1)); - for i=0:2 - P(c(1+i))=P(c(1+i))+nodeloads(load,2+i); - end - end - free=[]; %contains unconstrainted codenumbers - supported=[]; %contains constrainted codenumbers - for node=1:rows(joints) - c=node2code(node); - for i=3:5 - if (joints(node,i)==0) - free=[free,c(i-2)]; - else - supported=[supported,c(i-2)]; - end - end - end - - %% global equation - %% { P_f } [ K_ff K_fs ] { Delta_f } { P_F_f } - %% { } = [ ] . { } + { } - %% { P_s } [ K_sf K_ss ] { Delta_s } { P_F_s } - %% Solutions: - %% Delta_f = K_ff^-1 (P_f - P_F_f) - %% P_s = K_sf * Delta_f + P_F_s - - %stiffness matrix - [ K_ff, K_sf ] = GlobalStiffnessMatrixRegrouped (joints, members,free,supported); %K_ss, K_fs are not used and if not calculated script is faster - - - %nodal forces and equivalent nodal ones - [P_F,MemFEF]=EquivalentJointLoads(joints,members,dist,point); - - - %reduced matrices - P_f=P(free,:); - P_s=P(supported,:); - - P_F_f=P_F(free,:); - P_F_s=P_F(supported,:); - - %solution: find unknown displacements - - try - %A X = B => solve with cholesky => X = R\(R'\B) - r = chol (K_ff); - D_f=r\(r'\(P_f-P_F_f)); - catch - error("System is unstable because K_ff is singular. Please check the support constraints!"); - end_try_catch - - %TODO: make use of iterative solver as an option. How???? - - %[D_f, flag] = pcg(K_ff,P_f-P_F_f,1e-6,1000); - %if (flag==1) - %max iteration - % printf('max iteration'); - % try - % %A X = B => solve with cholesky => X = R\(R'\B) - % r = chol (K_ff); - % D_f=r\(r'\(P_f-P_F_f)); - % catch - % error("System is unstable because K_ff is singular. Please check the support constraints!"); - % end_try_catch - %end - %if (flag==3) - %K_ff was found not positive definite - % error("System is unstable because K_ff is singular. Please check the support constraints!"); - %end - - - - - D(free,1)=D_f; - - %solution: find unknown (reaction) forces - P_s=K_sf*D_f+P_F_s; - P(supported,1)=P_s; - - - %solution: find unknown member forces - MemF=[]; - for member=1:rows(members) - N=members(member,1); - F=members(member,2); - xN=joints(N,1); - yN=joints(N,2); - xF=joints(F,1); - yF=joints(F,2); - T=TransformationMatrix(xN,yN,xF,yF); - k=MemberStiffnessMatrix(sqrt((xN-xF)**2+(yN-yF)**2),members(member,3),members(member,4),members(member,5)); - c=[node2code(N),node2code(F)]; - MemF=[MemF;(k*T*D(c'))']; - end - MemF=MemF+MemFEF;%+fixed end forces - - %format codenumbers to real output - %TODO: not efficient enough due to A=[A;newrow] - Displacements=[]; - Reactions=[]; - for i=0:rows(joints)-1 - Displacements=[Displacements;D(1+3*i:3+3*i)']; - Reactions=[Reactions;P(1+3*i:3+3*i)']; - end - for i=1:rows(Reactions) - for j=1:columns(Reactions) - if (joints(i,j+2)==0) - Reactions(i,j)=NaN; - end - end - end -end
--- a/main/mechanics/ocframe_deprecated/Test.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,50 +0,0 @@ -## -*- texinfo -*- -## @deftypefn {Function File} {} ocframe_ex3() -## Example of a planar frame. -## -## @end deftypefn -function Test() - UNP120=[210e9,363.990e-8,17.000e-4]; - RHS=[210e9,28.900e-8,8.730e-4]; - joints=[0,0,1,1,0; - 1.25,0,0,0,0; - 1.575,0,0,0,0; - 2.5,0,0,0,0; - 3.425,0,0,0,0; - 3.75,0,0,0,0; - 5.,0,0,1,0; - 0,1.5,0,0,0; - 1.25,1.5,0,0,0; - 2.5,1.5,0,0,0; - 3.75,1.5,0,0,0; - 5,1.5,0,0,0]; - members=[1,2,UNP120; - 2,3,UNP120; - 3,4,UNP120; - 4,5,UNP120; - 5,6,UNP120; - 6,7,UNP120; - 8,9,UNP120; - 9,10,UNP120; - 10,11,UNP120; - 11,12,UNP120; - 1,8,RHS; - 2,9,RHS; - 4,10,RHS; - 6,11,RHS; - 7,12,RHS]; - - nodeloads=[3,0,-30e3,0; - 5,0,-30e3,0; - 12,30e3,0,0]; - tic - [P1,D1,MemF1]=SolveFrame(joints,members,nodeloads,[],[]); - toc - tic - [P2,D2,MemF2]=SolveFrameOpt(joints,members,nodeloads,[],[]); - toc - - P1-P2 - D1-D2 - MemF1-MemF2 -end
--- a/main/mechanics/ocframe_deprecated/test_FixedEndForcesDist.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,43 +0,0 @@ -## Copyright (C) 2010 Johan Beke -## -## This software is free software; you can redistribute it and/or modify it -## under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 3 of the License, or (at -## your option) any later version. -## -## This software is distributed in the hope that it will be useful, but -## WITHOUT ANY WARRANTY; without even the implied warranty of -## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -## General Public License for more details. -## -## You should have received a copy of the GNU General Public License -## along with this software; see the file COPYING. If not, see -## <http://www.gnu.org/licenses/>. - -function [F1,F2,F3,F4,F5,F6]=test_FixedEndForcesDist(l,FxN,FyN,FxF,FyF,a,b) - % distributed load in local coords. - % a: distance from Near - % b: distance from Far - % FxN, FyN forces on start of load in local coordinates - % FxF, FyF forces on end of load in local coordinates - - x=linspace(a,l-b,100); #generate 100 intervals for riemann integration - dx=x(2)-x(1); - S=[]; - for i=1:columns(x) - Fy = (FyF-FyN)/(l-b-a)*(x(i)-a)+FyN; - Fx = (FxF-FxN)/(l-b-a)*(x(i)-a)+FxN; - [ F1,F2,F3,F4,F5,F6 ] = test_FixedEndForcesPnt(l,x(i),Fx,Fy); - S=[S;[ F1,F2,F3,F4,F5,F6 ]]; - end - for i=1:columns(S) - S(1,i)/=2; - S(rows(S),i)/=2; - end - F1 = sum(S(:,1))*dx; - F2 = sum(S(:,2))*dx; - F3 = sum(S(:,3))*dx; - F4 = sum(S(:,4))*dx; - F5 = sum(S(:,5))*dx; - F6 = sum(S(:,6))*dx; -end
--- a/main/mechanics/ocframe_deprecated/test_FixedEndForcesPnt.m Fri Dec 09 11:34:59 2011 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,36 +0,0 @@ -## Copyright (C) 2011 Johan -## -## This program is free software; you can redistribute it and/or modify -## it under the terms of the GNU General Public License as published by -## the Free Software Foundation; either version 2 of the License, or -## (at your option) any later version. -## -## This program is distributed in the hope that it will be useful, -## but WITHOUT ANY WARRANTY; without even the implied warranty of -## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -## GNU General Public License for more details. -## -## You should have received a copy of the GNU General Public License -## along with Octave; see the file COPYING. If not, see -## <http://www.gnu.org/licenses/>. - -## test_FixedEndForcesPnt - -## Author: Johan <johan@johan-Satellite-L30> -## Created: 2011-10-02 - -function [ F1,F2,F3,F4,F5,F6 ] = test_FixedEndForcesPnt (l,a,Fx,Fy) - joints=[0,0,1,1,1; - a,0,0,0,0; - l,0,1,1,1]; - nodeloads=[2,Fx,Fy,0]; - members=[1,2,1,1,1; - 2,3,1,1,1]; - R=SolveFrame(joints,members,nodeloads,[],[]); - F1 = R(1,1); - F2 = R(1,2); - F3 = R(1,3); - F4 = R(3,1); - F5 = R(3,2); - F6 = R(3,3); -endfunction