Geometric Functions¶
Float-vectors¶
A float-vector is a simple vector whose elements are specialized to floating point numbers. A float-vector can be of any size. When result is specified in an argument list, it should be a float-vector that holds the result.
float-vector &rest numbers
makes a new float-vector whose elements are numbers. Note the difference between (float-vector 1 2 3) and #F(1 2 3). While the former create a vector each time it is called, the latter does when it is read.
float-vector-p obj
returns T if obj is a float-vector.
v+ fltvec1 fltvec2 &optional result
adds two float-vectors.
v- fltvec1 &optional fltvec2 result
subtract float-vectors. If fltvec2 is omitted, fltvec1 is negated.
v. fltvec1 fltvec2
computes the inner product of two float-vectors.
v* fltvec1 fltvec2 &optional result
computes the outer product of two float-vectors.
v.* fltvec1 fltvec2 fltvec3
computes the scaler triple product [A,B,C]=(V. A (V* B C))=(V. (V* A B) C).
v:math:`<` fltvec1 fltvec2
returns T if every element of fltvec1 is smaller than the corresponding element of fltvec2.
v:math:`>` fltvec1 fltvec2
returns T if every element of fltvec1 is larger than the corresponding element of fltvec2.
vmin &rest fltvec
finds the smallest values for each dimension in fltvec, and makes a float-vector from the values. Vmin and vmax are used to find the minimal bounding box from coordinates of vertices.
vmax &rest fltvec
finds the greatest values for each dimension in fltvec, and makes a float-vector from the values.
minimal-box v-list minvec maxvec &optional err
computes the minimal bounding box for a given vertex-list, and stores results in minvec and maxvec. If a floating number err is specified, the minimal box is grown by the ratio, i.e. if the err is 0.01, each element of minvec is decreased by 1% of the distance between minvec and maxvec, and each element of maxvec is increased by 1%. Minimal-box returns the distance between minvec and maxvec.
scale number fltvec &optional result
the scaler number is multiplied to the every element of fltvec.
norm fltvec
\(|fltvec|\)
norm2 fltvec
\(|fltvec|^2=({\bf v.} fltvec fltvec)\)
normalize-vector fltvec &optional result
normalizes fltvec to have the norm 1.0.
distance fltvec1 fltvec2
returns the distance \(|fltvec-fltvec2|\) between two float-vectors.
distance2 fltvec1 fltvec2
\(|fltvec-fltvec2|^2\)
homo2normal homovec &optional normalvec
A homogeneous vector homovec is converted to its normal representation.
homogenize normalvec &optional homovec
A normal vector normalvec is converted to its homogenous representation.
midpoint p p1 p2 &optional result
is float, and p1 and p2 are float-vectors of the same dimension. A point \((1-p) p1 + p p2\), which is the point that breaks p1-p2 by the ratio \(p:(1-p)\), is returned.
rotate-vector fltvec theta axis &optional result
rotates 2D or 3D fltvec by theta radian around axis. Axis can be one of :x, :y, :z, 0, 1, 2 or NIL. When axis is NIL, fltvec is taken to be two dimensional. To rotate a vector around an arbitrary axis in 3D space, make a rotation matrix by the rotation-matrix function and multiply it to the vector.
Matrix and Transformation¶
A matrix is a two-dimensional array whose elements are all floats. In most functions a matrix can be of any size, but the v*, v.*, Euler-angle and rpy-angle functions can only handle three dimensional matrices. Transform, m* and transpose do not restrict the matrices to be square, and they operate on general n*m size matrices.
Functions that can accept result parameter places the computed result there, and no heap is wasted. All matrix functions are intended for the transformation in the normal coordinate systems, and not in the homogeneous coordinates.
The rpy-angle function decomposes a rotation matrix into three components of rotation angles around z, y and x axes of the world coordinates. The Euler-angle function is similar to rpy-angle but decomposes into rotation angles around local z, y and again z axes. Both of these functions return two solutions since angles can be taken in the opposite directions.
; Mat is a 3X3 rotation matrix.
(setq rots (rpy-angle mat))
(setq r (unit-matrix 3))
(rotate-matrix r (car rots) :x t r)
(rotate-matrix r (cadr rots) :y t r)
(rotate-matrix r (caddr rots) :z t r)
;--> resulted r is equivalent to mat
To keep track of pairs of a position and a orientation in 3D space, use the coordinates and cascaded-coords classes detailed in the section [Coordinates].
matrix &rest elements
makes a new matrix from elements. Row x Col = (number of elements) x (length of the 1st element). Each of elements can be of any type of sequence. Each sequence is lined up as a row vector in the matrix.
make-matrix rowsize columnsize &optional init
makes a matrix of \(rowsize \times columnsize\).
matrixp obj
T if obj is a matrix, i.e. obj is a two dimensional array and its elements are floats.
matrix-row mat row-index
extracts a row vector out of matrix mat. matrix-row is also used to set a vector in a particular row of a matrix using in conjunction with setf.
matrix-column mat column-index
extracts a column vector out of mat. matrix-column is also used to set a vector in a particular column of a matrix using in conjunction with setf.
m* matrix1 matrix2 &optional result
concatenates matrix1 and matrix2.
transpose matrix &optional result
transposes matrix, i.e. columns of matrix are exchanged with rows.
unit-matrix dim
makes an identity matrix of dim \(\times\) dim.
replace-matrix dest src
replaces all the elements of dest matrix with ones of src matrix.
scale-matrix scalar mat
multiplies scaler to all the elements of mat.
copy-matrix matrix
makes a copy of matrix.
transform matrix fltvector &optional result
multiplies matrix to fltvector from the left.
transform fltvector matrix &optional result
multiplies matrix to fltvector from the right.
rotate-matrix matrix theta axis &optional world-p result
multiplies a rotation matrix from the left (when world-p is non-nil) or from the right (when world-p is nil). When a matrix is rotated by rotate-matrix, the rotation axis :x, :y, :z or 0,1,2 may be taken either in the world coordinates or in the local coordinates. If world-p is specified nil, it means rotation along the axis in the local coordinate system and the rotation matrix is multiplied from the right. Else if worldp is non-nil, the rotation is made in the world coordinates and the rotation matrix is multiplied from the left. If NIL is given to axis, matrix should be two dimensional and the rotation is taken in 2D space where world-p does not make sense.
rotation-matrix theta axis &optional result
makes a 2D or 3D rotation matrix around axis which can be any of :x, :y, :z, 0, 1, 2, a 3D float-vector or NIL. When you make a 2D rotation matrix, axis should be NIL.
rotation-angle rotation-matrix
extracts a equivalent rotation axis and angle from rotation-matrix and a list of float and float-vector is returned. NIL is returned when rotation-matrix is a unit-matrix. Also if the rotation angle is too small, the result may have errors. When rotation-matrix is 2D, the single angle value is returned.
rpy-matrix ang-z ang-y ang-x
makes a rotation matrix defined by roll-pitch-yaw angles. First, a unit-matrix is rotated by ang-x radian along X-axis. Next, ang-y around Y-axis and finally ang-z around Z-axis. All the rotation axes are taken in the world coordinates.
rpy-angle matrix
extracts two triplets of roll-pitch-yaw angles of matrix.
Euler-matrix ang-z ang-y ang2-z
makes a rotation matrix defined by three Euler angles. First, a unit-matrix is rotated ang-z around Z-axis, next, ang-y around Y-axis and finally ang2-z again around Z-axis. All the rotation axes are taken in the local coordinates.
Euler-angle matrix
extracts two tuples of Euler angles.
LU decomposition¶
lu-decompose and lu-solve are provided to solve simultaneous linear equations. First, lu-decompose decomposes a matrix into a lower triangle matrix and an upper triable matrix. If the given matrix is singular, LU-decompose returns NIL, otherwise it returns the permutation vector which should be supplied to LU-solve. Lu-solve computes the solution for a LU matrix with a given constant vector. This method is efficient if solutions for many combinations of different constant vectors and the same factor matrix are required. Simultaneous-equation would be more handy when you wish to get only one solution. Lu-determinant computes a determinant of a lu-decomposed matrix. Inverse-matrix function computes an inverse matrix using lu-decompose once, and lu-solve n times. Computation time for a 3*3 matrix is estimated to be 4 milli-sec.
lu-decompose matrix &optional result
performs lu-decomposition of matrix.
lu-solve lu-mat perm-vector bvector &optional result
solves a linear simultaneous equations which has already been lu-decomposed. perm-vector should be the result returned by lu-decompose.
lu-determinant lu-mat perm-vector
computes the determinant value for a matrix which has already been lu-decomposed.
simultaneous-equation mat vec
solves a linear simultaneous equations whose coefficients are described in mat and constant values in vec.
inverse-matrix mat
makes the inverse matrix of the square matrix, mat.
pseudo-inverse mat
computes the pseudo inverse matrix using the singular value decomposition.
Coordinates¶
Coordinate systems and their transformations are represented by the coordinates class. Instead of 4*4 (homogeneous) matrix representation, coordinate system in EusLisp is represented by a combination of a 3*3 rotation matrix and a 3D position vector mainly for speed and generality.
coordinates
defines a coordinate system with a pair of a position vector and a 3x3 rotation matrix.
coordinates-p obj
returns T when obj is an instance of coordinates class or its subclasses.
:rot **
returns the 3X3 rotation matrix of this coords.
:pos **
returns the 3-D position vector of this coords.
:newcoords newrot &optional newpos
updates the coords with newrot and newpos. Whenever a condition that changes the state of this coords occurs, this method should be called with the new rotation matrix and the position vector. This message may invoke another :update method to propagate the event. If newpos is not given, newrot is given as a instance of coordinate class.
:replace-coords newrot &optional newpos
changes the rot and pos slots to be updated without calling newcoords method. If newpos is not given, newrot is given as a instance of coordinate class.
:coords **
:copy-coords &optional dest
If dest is not given, :copy-coords makes another coordinates object which has the same rot and pos slots. If dest is given, rot and pos of this coordinates is copied to the dest coordinates.
:reset-coords **
forces the rotation matrix of this coords to be identity matrix, and pos vector to be all zero.
:worldpos **
:worldrot **
:worldcoords **
Computes the position vector, the rotation matrix and the coordinates of this object represented in the world coordinates. The coordinates class is always assumed to be represented in world, these method can simply return pos, rot and self. These methods are provided for the compatibility with cascaded-coords class which cannot be assumed to be represented in world.
:copy-worldcoords &optional dest
First, worldcoords is computed, and it is copied to dest. If no dest is specified, a coordinates object to store the result is newly created.
:rotate-vector vec
A vector is rotated by the rotation of this coords, i.e., an orientation vector represented in this coords is converted to the representation in the world. The position of this coords does not affect rotation.
:transform-vector vec
A vector in this local coords is transformed to the representation in the world.
:inverse-transform-vector vec
A vector in the world is inversely transformed to the representation in this local coordinate system.
:transform trans &optional (wrt :local)
Transform this coords by the trans represented in wrt coords. Trans must be of type coordinates, and wrt must be one of keywords :local, :parent, :world or an instance of coordinates. If wrt is :local, the trans is applied from the right to this coords, and if wrt is :world or :parent, the trans is multiplied from the left. Else, if wrt is of type coordinates, the trans represented in the wrt coords is first transformed to the representation in the world, and it is applied from the left.
:move-to trans &optional (wrt :local)
Replaces the rot and pos of the coords with trans represented in wrt.
:translate p &optional (wrt :local)
changes the position of this object relatively with respective to wrt coords.
:locate p &optional (wrt :local)
Changes the location of this coords with the parameter represented in wrt. If wrt is :local, then the effect is identical to :translate with wrt=:local.
:rotate theta axis &optional (wrt :local)
Rotates this coords relatively by theta radian around the axis. Axis is one of axis-keywords (:x, :y and :z) or an arbitrary float-vector. Axis is considered to be represented in the wrt coords. Thus, if wrt=:local and axis=:z, the coordinates is rotated around the z axis of this local coords, and wrt=:world or :parent, the coords is rotated around the z axis of world coords. In other words, if wrt=:local, a rotation matrix is multiplied from the right of this coords, and if wrt=:world or :parent, a rotation matrix is multiplied from the left. Note that even wrt is either :world or :parent, the pos vector of this coordinates does not change. For the true rotation around the world axis, an instance of coordinates class representing the rotation should be given to :transform method.
:orient theta axis &optional (wrt :local)
forces setting rot. This is an absolute version of :rotate method.
:inverse-transformation **
makes a new coords that is inverse to self.
:transformation coords (wrt :local)
makes the transformation (an instance of coordinates) between this coords and the coords given as the argument. If wrt=:local, the result is represented in local coords, i.e., if the resulted transformation is given as an argument to :transform with wrt=:local, this coords is transformed to be identical with the coords.
:Euler az1 ay az2
sets rot with Euler angles, that are, rotation angles around z (az1, y (ay) and again z az2 axis of this local coordinates system.
:roll-pitch-yaw roll pitch yaw
sets rot with roll-pitch-yaw angles: rotation angles around x (yaw), y (pitch) and z (roll) axes of the world coordinate system.
:4x4 &optional mat44
If a matrix of 4x4 is given as mat44, it is converted to coordinates representation with a 3x3 rotation matrix and a 3D position vector. If mat44 is not given, this coordinates is converted to 4x4 matrix representation.
initializes this coordinates object and sets rot and pos. The meaning of each keyword follows:
- :dimension
- 2 or 3 (default is 3)
- :pos
- specifies a position vector (defaulted to #f(0 0 0))
- :rot
- specifies a rotation matrix (defaulted to a unit-matrix)
- :euler
- gives a sequence of three elements for Euler angles
- :rpy
- gives a sequence of three elements for roll-pitch-yaw
- :axis
- rotation axis (:x,:y,:z or an arbitrary float-vector)
- :angle
- rotation angle (used with :axis)
- :wrt
- where the rotation axis is taken (default is :local)
- :4X4
- 4X4 matrix is used to specify both pos and rot
- :coords
- copies pos and rot from coords
- :name
- set :name property
:Angle can only be used in conjunction with the :axis that is determined in the :wrt coordinates. Without regard to :wrt, :Euler always specifies the Euler angles, az1, ay and az2, defined in the local coordinates, and :rpy specifies the angles around z, y and x axes of the world coordinates. Two or more of :rot, :Euler, :rpy, :axis and :4X4 cannot be specified simultaneously, although no error is reported. Sequences can be supplied to the :axis and :angle parameters, which mean successive rotations around the given axes. List of pairs of an attribute and its value can be given as :properties argument. These pairs are copied in the plist of this coordinates.
CascadedCoords¶
cascaded-coords
defines a linked coordinates. Cascaded-coords is often abbreviated as .
:inheritance **
returns the inheritance tree list describing all the descendants of the cascoords. If a and b are the direct descendants of this coords, and c is a descendant of a, ((a (c)) (b)) is returned.
:assoc childcoords &optional relative-coords
is associated to this cascoords as a descendant. If childcoords has been already assoc’ed to some other cascoords, first childcoords is dissoc’ed since each cascoords can have only one parent. The orientation or location of childcoords in the world does not change.
:dissoc childcoords
dissociates (removes) childcoords from the descendants list of this coords. The orientation or location of childcoords in the world does not change.
:changed **
informs this coords that the coordinates of parent has changed, and the re-computation of worldcoords is needed when it is requested later.
:update **
is called by the :worldcoords method to recompute the current worldcoord.
:worldcoords **
returns a coordinates object which represents this coord in the world by concatenating all the cascoords from the root to this coords. The result is held in this object and reused later. Thus, you should not modify the resulted coords.
:worldpos **
returns rot of this coordinates represented in the world.
:worldrot **
returns pos of this coordinates represented in the world.
:transform-vector vec
Regarding vec represented in this local coords, transforms it to the representation in the world.
:inverse-transform-vector vec
represented in the world is inversely transformed into the representation in this local coords.
:inverse-transformation **
makes an instance of coordinates which represents inverse transformation of this coord.
:transform trans &optional (wrt :local)
:translate fltvec &optional (wrt :local)
:locate fltvec &optional (wrt :local)
:rotate theta axis &optional (wrt :local)
:orient theta axis &optional (wrt :local)
Refer to the descriptions in class coordinates.
make-coords &key pos rot rpy Euler angle axis 4X4 coords name
make-cascoords &key pos rot rpy Euler angle axis 4X4 coords name
coords &key pos rot rpy Euler angle axis 4X4 coords name
cascoords &key pos rot rpy Euler angle axis 4X4 coords name
All these functions make new coordinates or cascaded-coordinates. For the keyword parameter, see :init method of class coordinates.
transform-coords coords1 coords2 &optional (coords3 (coords))
Coords1 is applied (multiplied) to the coords2 from the left. The product is stored in coords3.
transform-coords* &rest coords
concatenates transformations listed in coords. An instance of coordinates that represents the concatenated transformation is returned.
wrt coords vec
transforms vec into the representation in coords. The result is equivalent to (send coords :transform-vector vec).
Relationship between transformation matrix and coordinates class¶
Relationship between transformation matrix and coordinates class is described, where a transformation matrix T represents a \(4\times4\)(homogeneous) matrix as below.
\(\mathbf{R}_T\) is a \(3\times3\) matrix, and \(\mathbf{p}_T\) is a \(3\times1\) matrix (a float-vector which has 3 elements in euslisp). Coordinates class has slot variables rot and pos. They are \(\mathbf{R}_T\) and \(\mathbf{p}_T\) respectively.
Getter method for rotation matrix and position¶
\(\mathbf{R}\) and \(\mathbf{p}\) can be obtained using methods of the coordinates class.
T is an instance of the coordinate class.
(send T :rot)
\(\Rightarrow\) \(\mathbf{R}_T\)
(send T :pos)
\(\Rightarrow\) \(\mathbf{p}_T\)
Methods for transforming vectors¶
\(\mathbf{v}\) is 3-D position vector.
(send T :rotate-vector :math:`mathbf{v}`)
\(\Rightarrow\) \(\mathbf{R}_T \mathbf{v}\)
(send T :inverse-rotate-vector :math:`mathbf{v}`)
\(\Rightarrow\) \(\mathbf{v}^T \mathbf{R}_T\)
(send T :transform-vector :math:`mathbf{v}`)
\(\Rightarrow\) \(\mathbf{R}_T\mathbf{v} + \mathbf{p}_T\)
Converts a vector represented in a local coordinate system T to a vector represented in the world coordinate system.
(send T :inverse-transform-vector :math:`mathbf{v}`)
\(\Rightarrow\) \(\mathbf{R}_T^{-1}\left( \mathbf{v} - \mathbf{p}_T \right)\)
Converts a vector represented in the world coordinate system. to a vector represented in a local coordinate system T.
Methods returing coordinates without modifying itself¶
(send T :inverse-transformation)
\(\Rightarrow\) \(T^{-1}\)
Returns inverse matrix.
(send T :transformation A (&optional (wrt :local)))
when wrt == :local, \(T^{-1}A\) is returned.
when wrt == :world, \(AT^{-1}\) is returned.
when wrt == W (coordinates class), \(W^{-1}AT^{-1}W\) is returned.
Methods modifying itself¶
A is an instance of the coordinates class.
\(\Leftrightarrow\) represents that slot variables (pos or rot) refer to a given instance (matrix or float vector). Please note that when one is changed, the other also reflects the change.
\(\leftarrow\) represents substitution.
(send T :newcoords A)
\(\mathbf{R}_T \Leftrightarrow \mathbf{R}_A\)
\(\mathbf{p}_T \Leftrightarrow \mathbf{p}_A\)
(send T :newcoords :math:`mathbf{R}` :math:`mathbf{p}`)
\(\mathbf{R}_T \Leftrightarrow \mathbf{R}\)
\(\mathbf{p}_T \Leftrightarrow \mathbf{p}\)
(send T :move-to A (&optional (wrt :local)))
when wrt == :local,:math:T leftarrow TA
when wrt == :world,:math:T Leftrightarrow A
when wrt == W (coordinates class), \(T \leftarrow WA\)
(send T :translate :math:`mathbf{v}` (&optional (wrt :local)))
when wrt == :local, \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{T} + \mathbf{R}_{T}\mathbf{v}\)
when wrt == :world, \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{T} + \mathbf{v}\)
when wrt == W (coordinates class), \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{T} + \mathbf{R}_{W}\mathbf{v}\)
(send T :locate :math:`mathbf{v}` (&optional (wrt :local)))
when wrt == :local, \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{T} + \mathbf{R}_{T} \mathbf{v}\)
when wrt == :world, \(\mathbf{p}_{T} \leftarrow \mathbf{v}\)
when wrt == W (coordinates class), \(\mathbf{p}_{T} \leftarrow \mathbf{p}_{W} + \mathbf{R}_{W}\mathbf{v}\)
(send T :transform A (&optional (wrt :local)))
when wrt == :local, \(T \leftarrow TA\)
when wrt == :world, \(T \leftarrow AT\)
when wrt == W (coordinates class), \(T \leftarrow\) \(\left( W A W^{-1} \right) T\)