Motion Control Library · Chapter 1: General Description 1.1 Introduction The Motion Library (ML) produces trajectories based on the PVT mechanism. It implements a set of functions

Motion Control Library Tutorial

January 2007 (Ver. 1.0)

Notice This tutorial is delivered subject to the following conditions and restrictions:

This tutorial contains proprietary information belonging to Elmo Motion Control Ltd.

The text and graphics included in this manual are for the purpose of illustration and reference only. The specifications on which they are based are subject to change without notice.

Elmo Motion Control and the Elmo Motion Control logo are trademarks of Elmo Motion Control Ltd.

Information in this document is subject to change without notice.

Document No. MAN-MLT Copyright ©2007

Elmo Motion Control Ltd. All rights reserved.

Revision History:

Ver. 2.0 Jan. 2007 (MAN-MLT.pdf) Updates

Ver. 1.0 June 2004 (MAN-MLT.pdf) Initial Release

Elmo Motion Control Inc. 1 Park Drive, Suite 12 Westford, MA 01886 USA Tel: +1 (978) 399-0034 Fax: +1 (978) 399-0035

Elmo Motion Control GmbH Steinkirchring 1 D-78056, Villingen-Schwenningen Germany Tel: +49 (07720) 8577-60 Fax: +49 (07720) 8577-70

www.elmomc.com

http://www.elmomc.com/

www.elmomc.com

Contents Chapter 1: General Description ..............................................................................................1

1.1 Introduction ............................................................................................................ 11.2 Vector properties ...................................................................................................... 11.3 Trajectory generation ............................................................................................... 3

1.3.1 Line................................................................................................................ 31.3.2 Circle ............................................................................................................. 31.3.3 Spline............................................................................................................. 3

1.3.3.1 Examples for the two-dimensional spline interpolation .............51.3.3.2 Examples of three-dimensional spline interpolation ...................6

1.3.4 Polyline ............................................................................................. 101.3.4.1 Examples for the two-dimensional polyline ...............................121.3.4.2 Examples for the three-dimensional polyline.............................14

1.4 Transition to a new trajectory with a non-zero velocity..................................... 16Chapter 2: Switch Radius Calculation ................................................................................2-1

2.1 Line – line intersection .......................................................................................... 2-12.2 Circle – line intersection ....................................................................................... 2-6

2.2.1 Line goes inside the circle ............................................................... 2-62.2.1.1 Switch arc center and circle center belong to two different half planes defined by the line L ................................................................... 2-62.2.1.2Switch arc center and a circle center belong to the same half plane………............................................................................................ 2-122.2.1.3 Line intersects the center of the circle...................................... 2-17

2.2.2 Switch arc radius calculation by the distance from the intersection point …………………………………………………………………….2-20

2.2.2.1 Initial circle center and switch arc center belong to the same half-plane ............................................................................................... 2-202.2.2.2 Initial circle center and switch arc center belong to two half planes defined by the line L. ................................................................ 2-222.2.2.3 Circle center (Xc,Yc) Є L1 (line L1 intersects the center of the circle)…................................................................................................... 2-24

2.2.3 Line goes outside the circle .................................................................. 2-242.2.3.1 Line L and init radius continued in their positive intersecting directions................................................................................................ 2-25

2.2.3.2 Line parallel to the circle arc init radius....................................... 2-272.2.3.3 Line L and init radius continued in their reverse directions intersect2-28

2.3 Circle – circle intersection.................................................................................. 2-302.3.1 One of two circle arcs intersects the internal area of the second...... 2-312.3.2 Each circle intersects the internal area of the second......................... 2-392.3.3 No circle intersects the internal area of the other .............................. 2-42

Appendix A: Projection of a point on a line defined by the end points ........................ A-1 Appendix B: Coefficients of the line standard equation for the line defined by the end points ........................................................................................................................................B-1 Appendix C: Intersection point of two lines defined by the end points ....................... C-1 Appendix D: Circle – line intersection points ................................................................... D-1

Motion Library Tutorial MAN-MLT (Ver 2.0)

Chapter 1: General Description

1.1 Introduction

The Motion Library (ML) produces trajectories based on the PVT mechanism. It implements a set of functions that calculate trajectories as PVT tables for the vector motion. As a result of a call to the Motion Library functions, the user gets a PVT table for the requested trajectory. It supports two and three-dimensional vector motion.

A PVT table is a two or three-dimensional sequence of PVT points.

Each PVT point is defined by:

• position value

• velocity for this position

• time interval that is necessary to arrive from the current position to the position defined by the next PVT point

It supports

1. Single shape trajectories:

line (2D,3D)

circle (2D)

2. Trajectory built from an arbitrary set of pointes interpolated by the cubic spline (2D,3D)

3. Polyline trajectory that can include a number of single shapes:

line segments (2D,3D)

circle arc segments (2D)

spline points (2D,3D)

Inside polyline transition from one shape to another can be executed with a non-zero velocity. In this case, an additional element – switch circle arc-- is inserted between two shapes.

In the case of two-dimensional vector motion, switch arcs can be built for the line-line, line-circle, circle-line and circle-circle intersections. In the case of a three-dimensional polyline, switch arcs can be built for the line-line intersection.

1.2 Vector properties Geometry of trajectory is defined by the set of vector functions such as circle() or line(). The Velocity profile is also influenced by the set of the following parameters (vector properties):

maximum vector acceleration/deceleration (vac/vdc)

maximum vector velocity (vsp)

end velocity (vse)

Maestro Motion Library Tutorial MAN-INTUG (Ver. 1.7)

1

general trajectory time (vtt)

switch arc definitions (vsc, vsr, vsd)

admissible velocity and position errors definitions (vpe,vve)

PVT step low and high limits (VNT,VXT )

All of the vector’s properties can be set in a user program or by the Command Interpreter. Syntax of a property: Vector_name.property

Examples:

v1.vsp - defines maximum vector velocity

v1.vtt - defines trajectory time

v1.vsc - defines smooth type from one shape to another

(1 – minimal radius switch arc, 2 – fixed radius switch arc, 3 – switch with a fixed distance from the intersection point).

Single shape trajectories can be executed in one of three modes pre-defined by the value of the input parameter vum: 1 – max velocity, 2 – fixed time, 3 – fixed velocity.

In the maximum velocity mode, velocity defined by the parameter vsp is considered a limiting value that cannot be exceeded. If a trajectory is not long enough to achieve such a value, then a trajectory with a triangle velocity profile is built and some maximum vector velocity Vmax < vsp is achieved at one point.

The fixed velocity mode (vum=3) is used if the user is interested in building a trajectory with a trapezium velocity profile – the main part of the trajectory (with the exception of possible acceleration/deceleration at the initial and final parts of the trajectory) is executed with a velocity equal to vsp. If a trajectory is not long enough to reach velocity vsp with the given vector acceleration /deceleration (input parameters vac/vdc), the trajectory is not built and the user receives an error message.

In the fixed time mode (vum = 2) the user must define parameter vtt – time in milliseconds for the trajectory execution. The Motion Library chooses a velocity profile that satisfies parameter vtt. If a trajectory with the given length, maximum velocity vsp and vector acceleration/deceleration cannot be executed within time vtt trajectory is not built and the user receives an error message.

The user can set values for the maximum PVT step in milliseconds – parameter vxt and for the minimal PVT step – parameter vnt ≥ 1msec. In this case, the main part of the trajectory will be executed with the PVT step ΔT = 0.5(vxt + vnt).

For the switch arc, connecting two shapes can be chosen from one of the three possible modes pre-defined by the input parameter vsc: 0 – no switch arc to be built, 1 – switch arc with the minimal possible radius, 2 – switch arc with radius pre-defined by the user, 3 – user defined distance from the intersection point (for the line-line or circle-line intersections).


2

1.3 Trajectory generation

1.3.1 Line

Target position for a line is defined by the parameters of the function line():

Two-dimensional line

V1.line(x,y) – produces a line trajectory from the current position to the point (x,y), where x and y integer values in counts.

Three-dimensional line

V1.line(x,y,z) – produces line trajectory from the current position to the point (x,y,z), where x,y and z - integer values in counts.

1.3.2 Circle

Radius, initial and sweep angles for a circle must be defined as parameters of a function circle(): V1.circle(radius, init_angle,sweep_angle), where init_angle and sweep_angle must be set in degrees (float), radius in counts (integer). Example (Motion Mathematic Lib Samples\ Vector_2D \CircleArc – www.elmomc.com) v1.vac = 28000000 //max acceleration

v1.vdc = 28000000 //max deceleration

v1.vum = 1 //build trajectory in max. velocity mode

v1.vsp = 250000 //maximum velocity

v1.vse = 0 //end velocity

v1.circle(100000,45,-270) //build circle arc trajectory

v1.bg // start motion

while (a1.ms==2)||(a2.ms==2) //wait until both axes have stopped

wait(10)

end while

1.3.3 Spline

A spline gives the possibility to move a smooth curve through an arbitrary set of points that do not necessary belong to a particular geometric shape as a circle, ellipse or a line.

The spline that is supported by the Motion Library is an interpolation cubic spline. All the points Po,P1,...,Pn given by the user belong to the spline curve. Between each pair of the neighboring points (Pi,Pi+1), cubic spline is defined by a third-order polynomial.


3

Other popular types of splines like Bezier curves, B- splines or NURBS are usually not interpolation but smoothing splines. The spline curve does not move through the given points but near them.

So cubic splines are piecewise polynomial (built of cubic polynomials for each segment [i,i+1]) and produce a curve with continuous first and second derivatives at the internal control points P1, P2, …, Pn-1 that, in case of motion control, means continuity of the velocity and acceleration. Denote a cubic spline polynomial for the segment [i,i+1] as Si(t) than Si(ti) = Si+1(ti), Si′(ti) = Si+1′(ti) and Si′′(ti) = S′′ i+1(ti), i = 1,2,…,n-1. The spline trajectory is executed in the maximum velocity mode vum = 1. Input parameters that define kinematics of the trajectory are maximum velocity – parameter vsp, vector acceleration – parameter vac and vector deceleration – parameter vdc.

The Motion Library user can define points for the spline interpolation applying the following function calls:

vector_name.splines(trj_name) - starts a sequence of points to be interpolated. The PVT table is saved in a file named trj_name. The function parameter trj_name can be missed. In this case, the trajectory is saved in a temporary file named vector_name.trj (where vector_name – name defined in a resource file).

To interpolate two-dimensional points, use a function call

vector_name.splinep(int PosX, int PosY) ) – adds interpolation point with coordinates (PosX, PosY).

In three-dimensional space

vector_name.splinep(int PosX, int PosY, int PosZ) ) – adds 3D interpolation point with coordinates (PosX, PosY, PosZ).

vector_name.splinee(parameter) - ends the spline trajectory sequence.

– If parameter = 0 the standard PVT table that in each line contains PVT points for the X and Y axes (for 2D spline) or PVT points for X, Y and Z axes (for 3D spline) is built.

– If parameter ≠ 0 three tables for the axis X, axis Y and for the gear are built. The table for the gear contains PVT points with the position equal to the distance along the spline from the initial spline position, vector velocity V = [Vx2 + Vy2]1/2 for this position and a standard time step (in 3D case four tables for X,Y, Z and gear are built).

Inside the spline operator parenthesis splines(trj_name) and vector_name.splinee(parameter) can be added operators for the position calculation (for instance ellipse points X = Xc + acos(t), Y = Yc + bsin(t)).

The Motion Library generates a trajectory by the cubic spline interpolation

Important note: Current position is not automatically added to the sequence of spline points. Homing must be done to the first spline point.


4

1.3.3.1 Examples for the two-dimensional spline interpolation

Example Example (Motion Mathematic Lib Samples\ Vector_2D \ Spline_Ellipse – www.elmomc.com)

Ellipse trajectory (2D spline interpolation)

v1.vum=1 //build trajectory in max velocity mode

v1.vac = 28000000 //max acceleration

v1.vdc = 28000000 //max deceleration

v1.vsp = 50000 //max. velocity

v1.vse = 0 //end velocity

pi = 3.14159265358979

a = 100000 //ellipse axis a

b = 50000 //ellipse axis b

Xc = 0 // ellipse center coordinate by X

Yc = 0 // ellipse center coordinate by Y v1.splines() // start spline trajectory

for teta = 0:pi/18:2*pi

x = Xc + a*cos(teta)

y = Yc + b*sin(teta)

v1.splinep(x,y) // add point to the spline trajectory

end for

v1.splinee(0) // end spline trajectory v1.bg //start motion

After homing drives must at the first point of the sequence.

Example Example (Motion Mathematic Lib Samples\ Vector_2D \ Lissajous curves – www.elmomc.com)

v1.vac = 28000000 //max vector acceleration

v1.vdc = 28000000 //max vector deceleration

v1.vsp = 100000 //set max. velocity

v1.vse = 0 //set end velocity

v1.splines() // start spline sequence


5


6

for t = 0:pi/72:2*pi

x = R*cos(3*t)

y = R*sin(5*t)

v1.splinep(x,y) // add spline point

end for

v1.splinee(0) // end spline sequence

v1.bg //start motion

while (a1.ms==2)||(a2.ms==2) //wait until both axes have stopped

wait(10)

end while

Figure 1-1: Recording of the spline trajectory (Lissajous curves)

1.3.3.2 Examples of three-dimensional spline interpolation

For 3D vector motion, there must be a defined resource file with a vector built from three axes. In the Elmo Studio it can be defined as shown in the picture below.


7

Figure 1-2: Resources for the 3D vector

Example (Motion Mathematic Lib Samples\ Vector_3D \ Spline_3D – www.elmomc.com)

A spline curve throws a number of arbitrary points

SetAxisStartPos(a1, 0) //set coordinate x to 0

SetAxisStartPos(a2, 0) //set coordinate y to 0


v2.vsp = 50000

v2.vse=0


v2.splinep(0, 0, 0) // add spline 3D point



v2.splinep(200000, 150000, 50000) //add spline 3D point


Three-dimensional picture can be drawn in Matlab with the use of the following Matlab operators

[n, posX, velX, posY, velY, posZ, velZ, t] = textread('D:\Dir_22_01\trj_file', '%d %d %d %d %d %d %d %d ', -1) where 'D:\Dir_22_01\trj_file – full path to the PVT table file.

For this operator to work properly, the first line of the PVT table containing a text header must be removed.

plot3(posX,posY,posZ)

axis square; grid on

Figure 1-3: Three-dimensional picture corresponding to the calculated

PVT table (drawn in Matlab)

Results of the recording

Figure 1-4: Projection on the XY plane


8

Figure 1-5: Projection on the XZ plane

Example (Motion Mathematic Lib Samples\ Vector_3D \ Helix – www.elmomc.com)

Helix curve built with the use of the spline interpolation.




v2.vsp = 50000

v2.vse=0

alpha = 0 //start angle degrees

beta = 3600 // sweep angle degrees

R = 50000

H = 250000 // height

X = 50000 // start pos x

Y = 0 // start pos y

Z = 0 // start pos z

n = 120 // num points

Teta = pi*(alpha/180) //radian

dTeta = (pi*(beta/180))/n // teta increment for one step

dZ = H/n //z increment for one step

Xc = X - R*cos(Teta) // X coordinate of the helix axis


9


10

Yc = Y - R*sin(Teta) // X coordinate of the helix axis


for i=0:1:n

v2.splinep(X,Y,Z) // add spline 3D point

Teta = Teta + dTeta // calc teta for the next point

X = Xc + R*cos(Teta) // calc X coordinate for the next point

Y = Yc + R*sin(Teta) // calc Y coordinate for the next point

Z = Z + dZ // calc Z coordinate for the next point

end for


v2.bg // begin motion

Figure 1-6: Helix three-dimensional picture for the calculated PVT table drawn in MATLAB

1.3.4 Polyline To build a polyline, the Maestro user program applies the following calls to the motion library functions:

vector_name.starts(trj_name) – starts the polyline trajectory sequence with saving the PVT table built by the Motion Library in a file named trj_name. Parameter trj_name can be missed – in this case trajectory is saved in a temporary file named vector_name.trj.

vector_name.ends() - ends the polyline trajectory sequence.

Inside the polyline operator parenthesis vector_name.starts(trj_name) and vector_name.ends() can be added function calls – addline(), addcircle(), addsplinep() and adddwell() to define polyline segments.

For the 2D polyline vector_name.addline (int PosX, int PosY) – adds a line segment

PosX, PosY - destination position of the linear segment (counts)

vector_name.addcircle(int Radius, float StartAngle, float SweepAngle) – adds circle arc segment

Radius - radius of the circle segment (counts)

start_angle - start angle of the circle segment (degrees)

sweep_angle - sweep angle of the circle segment (degrees)

vector_name.addsplinep(int x, int y) - adds two-dimensional spline point

Important note: The user should take into account that the last point of the previous segment or the first point of the trajectory is automatically added to the spline. The minimal number of the addsplinep() operators that define the same spline segment inside the polyline must be great equal 2. This requirement is valid for every spline segment inside the polyline – not only for the first one.

For the 3D polyline

vector_name.addline (int PosX, int PosY, int PosZ) – adds a line segment

PosX, PosY, PosZ - destination position of the linear segment (counts).

vector_name.addsplinep(int x, int y, int z) - adds three-dimensional spline point

As in a two-dimensional case, the last point of the previous segment is automatically added to the spline segment and the number of points defining the spline segment cannot be less than 2.

For 2D and 3D polyline

vector_name.adddwell(delay_time) – adding delay (station) between two segments

delay_time - delay value in millisecond

Smooth transition from one curve to another inside polyline.

There are four modes that define transition from one shape to another that are defined by the parameter (vector property) vsc:

1. vsc = 0 - switch arc is not built

2. vsc = 1 – ML builds switch arc with switch radius minimally radius that satisfies kinematics constraint r > (vse)2/[vac*vae] where

vse – parameter that defines end velocity on the segment preceding switch arc

vac – parameter that defines maximum vector acceleration

vae – parameter that defines admissible error for the vector acceleration


11

3. vsc = 2 – ML builds switch arc with the switch radius vsr

(this parameter must be set by the user).

4. vsc = 3 - ML builds a switch arc with the switch radius implicitly pre-defined via parameter vsd (distance along the line from the intersection point). Parameter vsd must be set by the user.

For vsc = 2 and vsc = 3, the user can check if the values of the parameters vsr and vsd satisfy geometric constraints. Such a check can be done with the use of algorithms described in chapter 2 of this document.

Switch arc building is also influenced by the previous segment end velocity defined by the parameter (vector property) vse.

1.3.4.1 Examples for the two-dimensional polyline

Example (Motion Mathematic Lib Samples\ Vector_2D \ LineCircle – www.elmomc.com)

v1.vac = 28000000 //max vector acceleration

v1.vdc = 28000000 // max vector deceleration

v1.vum = 1 // build trajectory in max velocity mode

v1.starts() // begin polyline trajectory

v1.vsp = 50000 // max velocity for the line segment

v1.vse = 50000 // end velocity

v1.addline(100000, 100000) // request to add line shape

v1.vse = 0 // end velocity for the circle segment

v1.vsc = 2 // smooth intersection with fixed switch radius

v1.vsr = 10000 // switch radius

v1.addcircle(50000,225,180) //request to add circle arc shape

v1.ends() // ends polyline trajectory


12



13

Figure 1-7: Recording of the polyline trajectory

Any trajectory generated by the Motion Library (single shape or polyline) can be rotated relative to init point due to vector property vra – rotation angle in degrees.

Example (Motion Mathematic Lib Samples\ Vector_2D \ LineCircleRotated – www.elmomc.com)

// The same polyline rotated at –90 degrees relative to init point

v1.vac = 28000000 //max vector acceleration v1.vdc = 28000000 // max vector deceleration v1.vum = 1 // build trajectory in max velocity mode v1.vra = -90 // rotate spline 90 degrees relative to init point

v1.starts() // begin polyline trajectory

v1.vsc = 2 // smooth intersection with fixed switch radius

v1.vsp = 50000 // max velocity for the line segment

v1.vse = 50000 // end velocity

v1.addline(100000, 100000) // request to add line shape

v1.vsp = 50000 // maximum velocity for the circle segment

v1.vse = 0 // end velocity for the circle segment

v1.vsr = 10000 // switch radius

v1.addcircle(50000,225,180) //request to add circle arc shape

v1.ends() // end polyline trajectory



14

Figure 1-8: Recording of the rotated polyline

1.3.4.2 Examples for the three-dimensional polyline

Example (Motion Mathematic Lib Samples\ Vector_3D \ Rectangle – www.elmomc.com)

Three-dimensional rectangle




v2.vsc=2

v2.vsr=12000

v2.vsp = 70000 //max. vector velocity

v2.vse = 70000

v2.starts()

v2.addline(50000, 50000, 0) //create line from current point to coordinate

v2.addline(50000, 50000, 50000)

v2.addline(0, 0, 50000)

v2.vse = 0

v2.addline(0, 0, 0)

v2.ends()



15

Figure 1-9: Three-dimensional polygon drawn in Matlab

Results of the recording

Figure 1-10: Projection on the XZ plane


16

Figure 1-11: Projection on the YZ plane

1.4 Transition to a new trajectory with a non-zero velocity

If a transition from one trajectory to another (for instance from a line to a circle) must be executed with a velocity not equal to zero at the switch point, the Motion Library builds a switch curve to achieve smooth modification of the velocity. Such a curve is implemented as a circle arc.

As a geometrical object switch arc is completely defined by:

• radius r

• coordinates of the two limit points (Xlast, Ylast) and (Xfirst, Yfirst)

where (Xlast, Ylast) – last point on the first shape

(Xfirst, Yfirst) – first point on the second shape

• center coordinates (Xo,Yo)

The radius r of the switch arc can be:

1. Explicitly pre-defined by the user (vsc = 2, vsr defined).

In this case, it must obey the following restriction:

r ≥ (Vend) 2/ACv (1-1)

where Vend - end velocity at the switch point,

ACv - vector acceleration.

In fact, the value defined as r ≥ (vse) 2/(vae*vac ) (by default vae = 0.9) must be used in the calculations.

2. Implicitly pre-defined by the user via smooth distance d along the line from the intersection point (vsc = 3, vsd defined). This mode can be implemented to the line – line, circle – line and line – circle trajectory intersections.

3. Calculated as a minimal possible for the given end velocity Vend and vector acceleration ACv (vsc = 1).

Regardless of the r definition (pre-defined by the user or calculated by (1-1)) movement along the switch arc is considered to be uniform with the constant tangent velocity V = Vend and constant angle velocity

ω = V/r (1-2)

The intersection geometry imposes constraints on the switch arc radius. So the switch radius in use r must satisfy

(Vend) 2/ACv < r < rmax (1-3)

Chapter 2 will consider all cases of the two shapes intersections and methods for the calculation of the switch radius limit value rmax.

As a base value for the switch velocity is taken the preceding segment end velocity Vend defined by the parameter vse. This value is considered as a limiting parameter that cannot be exceeded but can be decreased. It’s also valid in case that polyline segments (preceding switch arc and the following) are executed in the fixed velocity mode (vum = 3).

In switch mode vsc = 1, the initial value of a switch velocity equal vse and can be decreased to build a switch arc trajectory equal to the integer number of PVT steps or milliseconds.

In switch mode vsc = 1, the radius of the switch arc is calculated as a minimal possible for the intersection geometry and given vector acceleration/deceleration (input parameters vac/vdc) meaning that the calculated value satisfies (1-3). While the switch arc calculation is also taken into account the requirement to build switch arc trajectory equal to an integer number of PVT steps. The Motion Library is trying to build switch arc trajectory with velocity as close to the value given by the parameter vse for the preceding segment as possible. The switch arc (or two arcs on both ends of the segment) can take up almost all segment length that sometimes makes trajectory calculation impossible.

As an example consider the following: segment initial velocity equal Vo, end velocity Ve and

min(Vo,Ve)ΔT > ΔL (1-4)

(where ΔL – the length of the segment truncated by switch arcs, ΔT – standard PVT step). When defining switch radius (input parameter vsr) for vsc = 2 mode or end velocity (input parameter vse) for vsc = 1 mode, the user must take into account that it can significantly influences the whole trajectory and in fact replaces the main part of a particular segment by switch arcs.


17

Input parameters and intersection geometry define the influence of a switch arc on a trajectory. The main cases of shapes intersection are considered below. Here as an example to consider two lines intersection. If an angle between two lines is small, even a switch arc with a small radius can significantly change initial trajectory while an arc with the same radius can be insignificant for lines with intersection angle close to 180o.

In addition to geometric constraints, the Motion Library imposes limitations on the switch arc length. Each switch arc should not exceed 50% of the segment length. If there are two switch arcs adjacent to some polyline segment, then both arcs should not take more than 80% of its initial length.


18

Motion Library Tutorial Switch Radius Calculation MAN-MLT (Ver 2.0)

2-1

Chapter 2: Switch Radius Calculation

2.1 Line – line intersection

If a trajectory contains a switch from line L1 to line L2 with non-zero velocity, the direction of the vector velocity cannot be changed at the one intersection point. To implement such a switch, insert into a trajectory an additional element – circle arc is inserted into a trajectory (see-Figure 2-1). The switch arc radius must satisfy (1-1).

Figure 2-1

In the case of a line-line intersection, parameters r and d are connected by the simple equation

r = d*tg(γ/2) (2.1-1

where γ = π – α (α - angle between vectors L1 and L2) so the pre-defined parameter vsd = d must satisfy

vsd ≥ (Vend)2/[ACv tg(γ/2)] (2.1-2)

If ΔL1 is the length of the line L1 and ΔL2 is the length of the line L2, then there is an evident geometric constraint for the switch radius r

rmax ≤ min(ΔL1, ΔL2)*tg(γ/2) (2.1-3)

In fact, due to additional limitations (50% of the segment length) used in ML, the following should be used


2-2

vsr ≤ min(0.5ΔL1, 0.5ΔL2)*tg(γ/2) (2.1-4)

and kinematics constraint

Vend ≤ [r*vae*ACv)]1/2

(2.1-5)

where vae – acceleration error (default value – 0.9).

So the user defined parameters must satisfy

vse ≤ [vsr*vae*vac]1/2

(2.1-6)

vse – segment end velocity, vsr – switch radius, vac – vector acceleration.

Example 2.1a

Line 1 is defined by its init point (50000, 70000) and end point (60000,20000). .Line 2 is defined by the init point (60000,20000) and its end point (60000,70000). Switching from Line 1 to Line 2 must be executed with a minimal switch radius (vsc = 1). The cruise velocity is defined as vsp = 50000 and the end velocity vse = 50000. Vector acceleration/deceleration vac = vdc = 500000

1. The calculated minimal switch radius that satisfies kinematics constraint is

r_min = (vse)2/(vac*vae) = (50000)2/(500000*0.9) = 5555.6

2. The calculated distance from the intersection point that corresponds to r_min = 5555.6

ΔX1 = 60000 - 50000 = 10000, dY1 = 20000 – 70000 = -50000,

ΔX2 = 60000 – 60000 = 0, dY2= 70000 – 20000 = 50000

ΔL1 = [dX12 + dY12]1/2 = [(10000)2 + (-50000)2]1/2 = 50990

ΔL2 = [dX22 + dY22]1/2 = [0 + (50000)2]1/2 = 50000

γ = π – arcos[(ΔX1ΔX2 + ΔY1ΔY2)/(ΔL1ΔL2) =

= π – arcos{[(-50000)*(0) + (-50000)*(50000)]/(50990*50000)} = 0.1974

The distance from the intersection point corresponding to the minimal switch radius

d = r_min/tg(γ/2) = 5555.6/tg(0.5*0.1974) = 56105

d > ΔL1 and d > ΔL2 which means that the minimal switch radius does not fit the geometric constraints. Possible solutions: to decrease the end velocity vse or increase vector acceleration vac. Suppose that the vector acceleration is pre-defined by the mechanical parameters of the system and decrease end velocity.

dmax = min(ΔL1,ΔL2) = 50000


2-3

r_max = dmax*tg(γ/2) = 50000* tg(0.5*0.1974) = 4951

This value is limiting and produces singular trajectory with the switch arc that replaces L2. So we should take some value less than limiting value 4951 for instance 0.5* r_max = 0.5*4951 = 2475. Now we can recalculate maximum end velocity (vse) that satisfies this value:

vse = [r*vac*vae]1/2 = [2475*500000*0.9]1/2 = 33376

(any value less equal than 33376 can be used with the switch radius r = 2475).

Example 2.1b

Line 1 is defined by init point (50000, 70000) and end point (60000,20000). Line 2 is defined by the init point (60000,20000) and its end point (60000,70000). Switch from Line 1 to Line 2 must be executed with the pre-defined switch radius (vsc = 2). We define cruise velocity vsp = 50000 and end velocity vse = 50000. Vector acceleration/deceleration vac = vdc = 500000 and switch radius vsr = 6000

1. We calculated minimal switch radius that satisfies the kinematics constraint

r_min = (vse)2/(vac*vae) = (50000)2/(500000*0.9) = 5555.6

Pre-defined switch radius is greater than r_min so it satisfies kinematics constraints

2. We have to check if switch radius r_switch = 6000 satisfies geometric constraints

ΔX1 = 60000 - 50000 = 10000, dY1 = 20000 – 70000 = -50000,

ΔX2 = 60000 – 60000 = 0, dY2= 70000 – 20000 = 50000

ΔL1 = [dX12 + dY12]1/2 = [(10000)2 + (-50000)2]1/2 = 50990

ΔL2 = [dX22 + dY22]1/2 = [0 + (50000)2]1/2 = 50000

γ = π – arccos[(ΔX1ΔX2 + ΔY1ΔY2)/(ΔL1ΔL2) =

= π – arccos{[(-50000)*(0) + (-50000)*(50000)]/(50990*50000)} = 0.1974

The distance from the intersection point that corresponds to the r_switch = 6000

d = r_switch/tg(γ/2) = 6000/tg(0.5*0.1974) = 60593

The calculated value exceeds ΔL1 and ΔL2 meaning that the chosen switch radius does not fit geometric constraints and must be decreased:

r_max = min(ΔL1, ΔL2)tg(γ/2) = 50000*tg(0.5*0.1974) = 4951.

It is possible to choose any value r_switch that satisfies r_min < r_switch and r_switch < r_max but in this case r_min > r_max. So as r_switch, use 0.9min(r_min , r_max) = 4455.9.

The chosen value exceeds r_min so it does not fit the kinematics constraints and the end velocity must be decreased:


2-4

vse = [r_switch*vac*vae]1/2 = [4455.9*500000*0.9]1/2 = 44778.9

Example 2.1c

(Motion Mathematic Lib Samples \Line To Line\LineLineMinRad_Ex_2c - www.elmomc.com)

Line 1 is defined by its init point (300000, 900000) and end point (700000,200000). Line 2 is defined by the init point (700000,200000) and its end point (1100000,700000). Switch from Line 1 to Line 2 must be executed with the minimal switch radius (vsc = 1). We define the cruise velocity vsp = 50000 and the end velocity vse = 50000. Vector acceleration/deceleration is vac = vdc = 28000000.

1. We calculate the minimal switch radius that satisfies kinematics constraint by

r_min = (vse)2/(vac*vae) = (50000)2/(28000000*0.9) = 99.2

2. Geometric description of the two lines intersecting:

ΔX1 = X12 – X11 = 700000 - 300000 = 400000

ΔY1 = Y12 – Y11 = 200000 – 900000 = -700000

ΔX2 = X22 – X21 = 1100000 – 700000 = 400000

ΔY2 = Y22 – Y21 = 700000 – 200000 = 500000

The length of the first line segment

ΔL1 = [ΔX12 + ΔY1 2]1/2 = [4000002 + (-700000) 2]1/2 = 806225.8

The length of the second line segment

ΔL2 = [ΔX22 + ΔY12]1/2 = [4000002 + 5000002]1/2 = 640312.4

An angle between two lines can be calculated as

γ = π – arccos[(ΔX1ΔX2 + ΔY1ΔY2)/(ΔL1ΔL2) =

= π – arccos{[400000*400000 + (-700000)( 500000)]/(806225.8*640312.4)}= 1.193887 = 68.4o

The distance from the intersection point corresponding to the minimal switch radius

d = r_min/tg(γ/2) = 99.2*tg(0.5*1.193887)= 145.95

We see that d < ΔL1 and d < ΔL1 so the calculated switch radius satisfies the geometric constraints.

Example 2.1d

(Motion Mathematic Lib Samples\Line To Line\LineLineFixedDist_Ex_2d – www.elmomc.com)


2-5

Line 1 is defined by its init point (300000, 900000) and end point (700000,200000). Line 2 is defined by the init point (700000,200000) and its end point (1100000,700000). Switch from Line 1 to Line 2 must be executed with a distance from the intersection point pre-defined by the user vsc = 3, vsd = 20000. We define cruise velocity vsp = 50000 and end velocity vse = 50000. Vector acceleration/deceleration vac = vdc = 28000000

1. We calculate the minimal switch radius that satisfies the kinematics constraint

r_min = (vse)2/(vac*vae) = (50000)2/(28000000*0.9) = 99.2

2. Geometric description of the two lines intersecting:

ΔX1 = X12 – X11 = 700000 - 300000 = 400000

ΔY1 = Y12 – Y11 = 200000 – 900000 = -700000

ΔX2 = X22 – X21 = 1100000 – 700000 = 400000

ΔY2 = Y22 – Y21 = 700000 – 200000 = 500000

The length of the first line segment

ΔL1 = [ΔX12 + ΔY1 2]1/2 = [4000002 + (-700000) 2]1/2 = 806225.8

The length of the second line segment

ΔL2 = [ΔX22 + ΔY12]1/2 = [4000002 + 5000002]1/2 = 640312.4

Notice that d < ΔL1 and d < ΔL1

An angle between the two lines can be calculated as

γ = π – arcos[(ΔX1ΔX2 + ΔY1ΔY2)/( ΔL1ΔL2)] =

= π – arcos{[400000*400000 + (-700000)( 500000)]/(806225.8*640312.4)}= 1.193887 = 68.4o

The switch radius corresponding to the given d can be calculated as

r=d*tg(γ/2) = 20000tg(0.5*1.193887) = 13593

1. Check the kinematics constraints. A minimal switch radius that fits kinematics constraints can be calculated as

rmin > vse*vse/(vac*vae) = 500002/(28000000*0.9) = 99.2

The implicitly pre-defined switch radius is 13593 >> rmin.

A number of similar examples with a different set of user defined and calculated parameters are considered in the Motion Mathematic Lib Samples examples TwoLinesFixDist_Fx2D_1 - TwoLinesFixDist_Fx2D_2.


2-6

2.2 Circle – line intersection

Note: C – circle arc, L – line, R – circle radius, r – switch arc radius, (Xc,Yc) - circle center, (Xi, Yi) – intersection point, (Xlast, Ylast) – last point on the circle (Xfirst, Yfirst) – first point on the line L, d – distance from point (Xi, Yi) to point (Xfirst, Yfirst), (Xo,Yo) – switch arc center coordinates.

There are three possible cases that influence the calculation of parameters that define a switch arc: initial circle center and switch arc center belong to the same half-plane, initial circle center and switch arc center belong to different half-planes (defined by the line L), and when the line (or continued line) moves through the center of the initial circle. For each of these three cases, two sub cases are possible: from the point of intersection the line goes either outside or inside the circle.

Circle – line intersection geometry must satisfy some necessary conditions for the switch arc to be built.

On the first stage of calculations we define switch arc radius. It can be predefined by the user or calculated as V2/(vae*ACv), where V – end velocity and ACv vector acceleration, vae -admissible acceleration error. If the intersection was defined by the distance d than the switch arc radius can be calculated as described in 2.2.2

No matter how a switch arc radius was defined it must be coordinated with a circle and a line parameter

2.2.1 Line goes inside the circle

2.2.1.1 Switch arc center and circle center belong to two different half planes defined by the line L

The switch arc radius must obey

(vse)2/(vac*vae) < vsr < (R – h)/2 (2.2.1.1-1)

where R - radius of the initial circle, h – the length of the perpendicular dropped from the circle center on the line L, vsr - parameter that defines switch arc radius, vae – admissible acceleration error.

This condition is necessary but not always sufficient. It’s sufficient only in the case that the projection point P1 of the circle center on the line belongs to the line segment (P1 ∈ L) and the point of intersection of the continued perpendicular with the circle (point P2 in Figure 2-2) belongs to the circle arc (P2 ∈ C). If one of these conditions is not fulfilled, the maximum possible switch radius r must be re-calculated due to intersection geometry. Possible cases are considered below.


2-7

Figure 2-2

Example 2-2

(Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_1_1\

CircleLine_Ex_2_2 – www.elmomc.com)

Circle arc is defined by its init position Xfrom = 0, Yfrom = 100000, radius R = 100000, init angle α = 90o(with axis X positive direction), and sweep angle = 180o. Line end point Xto

= -140000, Yto = 100000.

Circle center coordinates

Xc = Xfrom – Rcos(απ/180) = 0 – 100000cos(π/2) = 0

Yc = Yfrom – Rsin(απ/180) = 100000 – 100000sin(π/2) = 0

Circle end points

X1 = Xc + Rcos(α + β) = 0 + 100000cos(π/2 + π) = 0

Y1 = Yc + Rcos(α + β) = 0 + 100000sin(π/2 + π) = -100000

Projection of the circle center on the line (Appendix )

K1 = (Y2 – Y1)/(X2 – X1) = (100000+100000)/(-140000-0)=-1.4286

K = -1/K1 = 0.7

Xp=(Yc – Y1 + K1*X1 - KXc)/(K1 - K) =

(0+100000 + 0.7*0 +1.4286*0))/(-1.4286 - 0.7) = -46979


2-8

Yp = Yc + K*(Xp – Xc) = 0 +0.7*(-46979 - 0) = -32885

And the perpendicular length

h = [(Xc – Xp)2 + (Yc - Yp)2]1/2 = [(0 + 46979)2 + (0 + 32885)2]1/2 = 57345

Max switch radius calculated by (2.2.1.1-1)

Rmax = (R – h)/2 = (100000 – 57345)/2 = 21328

Rmin = 1000002/(2800000*0.9) = 396.8

As a switch radius we can take any value Rswitch that satisfies

Rmin < Rswitch < Rmax

We take Rswitch = (Rmin + Rmax)/2 = (21328 + 396.8)/2 = 10862

In Figures 2-3 and 2-5, two possible cases are presented when the projection of the circle center on the line L does not belong to the line segment.

In Figure 2-3, perpendicular at the Line L end point P2 intersects with the circle C at point P3 that belongs to the circle arc P3 ∈ C. In Figure 2-3, observe the switch arc with the maximum possible radius r. This limit value can be calculated.

Figure 2-3

Since the coordinates of point P1(X1,Y1) – projection of the circle center point

O(Xc,Yc) on line L and line end point P2(X2,Y2) are known, the distance is

ρ(P1,P2) = u = [(X2 – X1)2 + (Y2 – Y1)2]1/2 (2.2.1.1-2)


2-9

The length of the perpendicular h should also be calculated. By knowing the line equation in a form

Ax + By + C = 0 (2.2.1.1-3)

then h can be calculated by

h = |AXc + BYc + C|/(A2 + B2)1/2 (2.2.1.1-4)

If line L is defined by its starting point (X1,Y1) and end point (X2,Y2), coefficients A,B,C can be calculated by formulas in Appendix 2 or Appendix 1.

To define r we can use an equation

(R – r)2 = (h + r)2 + u2 (2.2.1.1-5)

R2 – 2Rr + r2 = h2 + 2hr + r2 + u2 (2.2.1.1-6)

rmax = (R2 – h2 – u2)/(2R + 2h) (2.2.1.1-7)

This means that the parameter vsr that defines switch arc radius must obey the following constraints:

(vse)2/vac < vsr < (R2 – h2 – u2)/(2R + 2h) (2.2.1.1-8)

The switch arc with parameter vsr calculated by (2.2.1.1-7) takes up the whole line segment. To satisfy additional requirement that switch arc cannot take more than 50% of the segment length instead of the line end point we should use in calculations some other point P2′ that belongs to the line segment. The distance between this point and the intersection point ρ2 =

ρ(Pi,P2) must be equal k*l (where k < 0.5 , l – the length of the line). If the distance from the intersection point Pi to P1 is defined as ρ(Pi P1), then (2.2.1.1-7) can be presented as a function of the used partial length k*l of the line L.

rmax = [R2 – h2 – (ρ(Pi P1) – k*l)2]/(2R + 2h) (2.2.1.1-9)

Example 2-4



Circle arc (Figure 2-4) is defined by its init position Xfrom = 0, Yfrom = 0, radius R = 100000, init angle α = –90o(with axis X positive direction), and sweep angle = +60o. Line end point Xto = 25000, Yto = 50000. Circle end point [Xc + Rcos(-30), Yc + Rsin(-30)] = (86603, 50000) so the line L is parallel to the axis X. From Figure 2-4, h = 50000, u = 25000. The switch radius is calculated by (2.2.1.1-7).

r = 0.5*(1000002 – 500002 – 250002)/(100000 + 50000) = 22917


2-10

Figure 2-4

In our calculations was not taken in account additional requirement that the switch arc should not take more than 50% of the segment length. So the calculated switch arc with the radius 22917 takes the whole line segment.

If in the calculations, some point on the line is used, for example (65000, 50000), instead of the line end point, then u = 65000 and the result is:

r = 0.5*(1000002 – 500002 – 650002)/(100000 + 50000) = 10917

Perpendicular to the Line L end point P2 does not intersect the circle arc C

In Figure 2-5, there is an intersecting geometric figure showing that a perpendicular line at Line L end point P2 does not intersect the circle arc C. The switch arc drawn in Figure 2-5 is a switch arc with a maximum possible radius r.

By knowing the coordinates of a circle center O(Xc,,Yc) and a circle init point P6(X6,Y6) it is possible to find an intersection point P3 of two lines: (Xc,Yc) – (X6,Y6) and L and the length of the line segment P3P6: ρ1 = ρ(P3, P6).

Drop a perpendicular from point P6 on the line L and by (2.2.1.1-4) define its length

ρ2 = ρ(P6, P4). To define r, use a proportion

r/ρ2 = (ρ1 – r)/ρ1 (2.2.1.1-9)

and finally


2-11

r = ρ1ρ2/(ρ1 + ρ2) (2.2.1.1-10)

As in the previous case, the user defined parameter vsr must obey

(vse)2/vac < vsr < ρ1ρ2/(ρ1 + ρ2) (2.2.1.1-11)

Figure 2-5

Example 2-6



The circle arc in Figure 2-6 is defined by the radius R = 100000, init angle α = -90o, sweep angle β = 30o and the init position Xfrom = 0, Yfrom = -100000.

Line L end point: Xto = -40000, Yto = 0;

The circle arc end point P5 coordinates can be calculated as X5 = R*cos(π/3) = 50000, Y5 = R*sin(-π/3) = -86602.54;

Line L is defined by two points P1(-40000, 0) and P2[Rcos(-60), Rsin(-60)] =

P2(50000, -86602.54). Its standard equation is Ax + By + C = 0,

where A = – (Y2 – Y1) = -(0 + 86602.54) = –86602.54, B = X2 – X1 = -40000 – 50000 = -90000, C = X1(Y2 – Y1) – Y1(X2 – X1) = 50000(0 + 86602) + 86602.54(-40000 - 50000) =–3464101600.0 where (X1, Y1) and (X2,Y2) – line end points.

Calculate ρ2 by (3.1.1-4) as

ρ2 = |(-86602.54)0 + (-90000)(-100000) - 3464101600.0|/[(-86602.54)2 +(-90000)2 ]1/2 = 44322.6596

To define ρ1, take into account that line (O, P6) in Figure 2-6 coincides with the axis Y so coordinates of its intersection point with the line L are (0, -C/B).


2-12

ρ1= 100000 - |C/B| = 100000 - |(-3464101600.0)/(-90000)| = 61509.98222

r = ρ1ρ2/(ρ1 + ρ2) = 61509.98*44322.66/(61509.98 + 44322.66)= 25760.351 meaning that the switch radius in use must be less than this limiting value.

Figure 2-6

Values calculated by (2.2.1.1-7) and (2.2.1.1-10) are not recommended but limiting values. The switch radius in use must be less those values.

Such limiting values produce irregular cases of intersection. If we use (2.2.1.1-7) switch arc replaces line L and if we use (2.2.1.1-9) switch arc replaces initial arc C.

2.2.1.2Switch arc center and a circle center belong to the same half plane.

In this case (Figure 2-7), the switch radius must satisfy the following condition:

r ≤ (R + h)/2 (2.2.1.2-1)


2-13

Figure 2-7

This condition is not always sufficient. Adequacy depends on arc and line lengths. If the circle’s center projection on the line L belongs to the line segment (P1 ∈ L) and an intersection point of the continued perpendicular with the circle arc belonging to the circle segment P2 ∈ C then (2.2.1.2-1) is sufficient.

If projection of the circle init point P1 belongs to the line segment - P5 ∈ L (Figure 2-8), use the following proportion

h/r = ρ(P3, Oc)/[ρ(P1, P3) – r] = ρ1/(ρ2 – r) (2.2.1.2-2)

where P3 is the point of intersection of the line (P1,Oc) with the line L. As init point of the circle P1 and a circle center are known, P3 can be determined. The length h of the perpendicular dropped from the circle center Oc on the line L can be also found by (2.2.1.2-4) or another method. From (2.2.1.2-2) the results are

hρ2 – hr = rρ1 => r = hρ2/(ρ1 + h) (2.2.1.2-3)


2-14

Figure 2-8

Example 2-9



The circle arc (Figure 2-9) is defined by its radius R = 100000, init angle α = 0o and sweep angle β = -45o, init point P1(100000, 0). Line target position (-40000, 0).

In this case ρ1 = 40000, ρ2 = 140000 - line (P1,P3) coincide with axis X.

Circle center coordinates Xc = 100000 - Rcos(0) = 0, Yc = 0 - Rsin(0) 0.

Line init point coordinates X1 = 0 + 100000cos(0 - 45) = 70711, Y1 = 0 + 100000sin(0- 45) = -70711

To calculate the length of perpendicular h dropped from the circle center (Xc,Yc) on the line, use formulas (a1.6), (a1.4) and (a1.7) from Appendix 1.

k = (Y2-Y1)/(X2-X1)=(0+70711)/(-40000-70711)= -0.6387, q = -1/k = 1.5657

Xpr = (y - Y1 + k*X1 - q*x)/(k - q) = -11589.6, Ypr = y + q*(Xpr - x) = - 18145.7

h= [(-11589.6 - 0)2 + (- 18145.7 - )2]1/2 = 21531

r = hρ2/(ρ1 + h) = (21531*140000)/( 40000 + 21531) = 48989


2-15

Figure 2-9

Projection of the circle arc init point P1 on the line L does not belong to the line segment L.

If a projection of the circle arc init point P1 on the line L does not belong to the line segment L (Figure 2-10), calculate the maximum switch radius r using equation

ρ2(Oc,P5) + (r – h)2 = (R – r)2 (2.2.1.2-4) or

(ρ1)2 + (r – h)2 = (R – r)2 (2.2.1.2-5)

(ρ1)2 + r2 – 2rh + h2 = R2 – 2Rr + r2 (2.2.1.2-6)

R2 – (ρ1)2 – h2 = 2Rr – 2rh (2.2.1.2-7)

The results for r

r = [R2 – (ρ1)2 – h2]/(2R – 2h) (2.2.1.2-8)

To define ρ(Oc,P5), drop a perpendicular from the circle center Oc on the line L. The distance from the projection point P4 to the line end point P3 is equal to ρ(Oc,P5). As well as in case when the circle and switch arc centers belong to different half planes values calculated by (2.2.1.2-3) and (2.2.1.2-8) are not recommended but limiting ones. Switch radius in use must be less those values.


2-16

Figure 2-10

Example 2-11



Circle arc (Figure 2-11) is defined by its init position (X = 0, Y = 60000), radius R = 60000, init angle α = 0o, and a sweep angle β = -135o. Line target position 25000, -42426.

Circle center coordinates (Xc, Yc) = (0 – Rcos90, 60000 – Rsin90) = (0,0). Circle end point coordinates (Xc + Rcos(-45o), Yc + Rsin(-45o) = (42426, - 42426). Notice that line L is parallel to the axis X meaning that h = |0 - 42426| = 42426, ρ =|25000 - 0| =25000 and r can be calculated by

(3.1.2-8): r = (60000*60000 – 25000*25000 – 42426*42426)/(2*60000 – 2*42426) = 33430

Figure 2-11


2-17

2.2.1.3 Line intersects the center of the circle

Consider the last case of the circle – line intersection: the line goes inside the circle through the center of the circle (Figure 2-12-2-15). The following cases are possible:

a) The length of the line ΔL is greater than the circle radius R and the circle diameter orthogonal to the line intersects the circle arc: P1 ∈ C1 (Figure 2-12). In this case an evident geometric constraint on the switch arc radius is

r ≤ R/2 (2.2.1.3-1)

Figure 2-12

b) The length of the line ΔL is less than the circle radius R and a perpendicular at the line end point intersects the circle arc: P1 ∈C1 (Figure 2-13). To define the geometric limit for the switch radius r, use the following equation:

(R – r)2 = (R – ΔL)2 + r2 (2.2.1.3-2)

or

– 2Rr = – 2RΔL + ΔL2

that leads to

r = (2RΔL – ΔL2)/2R = ΔL – ΔL2/(2R) (2.2.1.3-3)


2-18

Figure 2-13

Example 2-14



The circle C1 (Figure 2-14) is defined by its init point (80000,0), radius R = 80000, init angle α = 0 and a sweep angle β = -135. Line target position (-28284, -28284). The length of the line segment ΔL = 28284*21/2= 40000.

The circle C1 is defined by the init point (80000,0), radius R = 80000, init angle α = 0 and a sweep angle β = 135. Circle target position: Xend = 80000*cos(135) = - 56569, Yend = 80000*sin(-135) = -56569.

The length of the line segment ΔL = [(-56569 + 28284)2 * 2]1/2= 40000.

Calculate the upper limit of the switch radius by (3.1.3-3): r = (2RΔL – ΔL2)/(2R) = (2*80000*40000- 400002)/(160000) =30000

Figure 2-14


2-19

c) The circle arc sweeps an angle less than 90o and a perpendicular dropped from the circle init point P1 on the line L intersects line segment at point P 2 ∈ C1 (Figure 2-15). A projection of the circle init point P1(X1,Y1) on the line L – point P2(X2,Y2) and the length of the segment defined by the line init point P3 and projection point P2: ρ1 = ρ(P3,P2) can be determined. Now calculate the maximum switch radius with the use of the following equation:

(R – r)2 = (R – ρ1)2 + r2

(2.2.1.3-4)

that leads to

–2Rr = – 2Rρ1 + (ρ1)2

and finally

r = [2Rρ1 – (ρ1)2]/(2R) (2.2.1.3-5)

Figure 2-15

Example 2-16



The circle (Figure 2-16 ) is defined by its init point P1(0, -80000), radius R = 80000, init angle α = -90o and sweep angle β =-45o. Coordinates of the line end point (150000,150000).

Circle end point P3 (line init point) coordinates are calculated as X3 = 80000*cos(pi + pi/4) = -56569 and

Y3 = 80000*sin(pi + pi/4) = -56569

Drop a perpendicular from the circle init point P1 on the line L2. To define coordinates of the projection point P2 , use formulas from the Appendix 1.

k = dY/dX = (150000 + 56569)/ (150000 + 56569) = 1, q = –1/k = –1.


2-20

By (a1.6) we have

Xp = (Yo – Y1 + kX1 – qXo)/(k – q) = (–80000 + 56569 – 56569 - 0)/(1+1) = –40000

Yp = Yo + q(X – Xo) = -80000 – (–40000 - 0) = –40000.

Distance ρ1 = ρ(P3,P2) = [(–40000 + 56569)2 + (–40000 + 56569)2]1/2 = 23432

For the maximum switch radius we get from (3.1.3-5)

r = [2Rρ1 – (ρ1)2]/(2R) = [2*80000*23432 - 234322]/160000 = 20000

Figure 2-16

2.2.2 Switch arc radius calculation by the distance from the intersection point

If svc = 3 mode (vsd = d is given) is considered and it is important to know the switch arc radius r to check if end velocity and vector acceleration satisfy (1-1). If d – distance from the point (Xi,Yi) to the point (Xfirst,Yfirst) is given, then it can be useful to re-calculate r as a function of parameters d and R (we have to know r to check condition 1-1).

Consider three possible cases of a circle and switch arc positions relative to the line.

2.2.2.1 Initial circle center and switch arc center belong to the same half-plane

2.2.2.1.1 Line continues outside the circle (Figure 2-17)

As in case of the switch arc center coordinates calculation we drop a perpendicular from the circle center (Xc,Yc) on the line and get a projection point (Xp,Yp). The length of the perpendicular ρ1 can be defined as

ρ1 = [(Xp – Xc)2 + (Yp – Yc)2]1/2 (2.2.2.1.1-1)

Define point (X1,Y1) so that


2-21

ρ[(Xp,Yp),(X1,Y1)] = r (2.2.2.1.1-2)

The following is known:

ρ3 = ρ[(Xp,Yp),(Xf, Yf)] = ρ[(Xp,Yp),(Xi, Yi)] + d (2.2.2.1.1-3)

where (Xi, Yi) – circle-line intersection point .

As ρ[(X1,Y1),(Xo, Yo)] = ρ[(Xp,Yp),(Xf, Yf)] = ρ3 then use an equation to define r

(R + r)2 – (ρ3)2 = (ρ1 – r)2

(2.2.2.1.1-4)

From (2.2.2.1.1-4) for r there is

r = [(ρ3)2 + (ρ1)2 – R2]/(2R + 2ρ1) (2.2.2.1.1-5)

2.2.2.1.2 Line continues inside the circle (Figure 2-18)

The following is known:

ρ3 = ρ[(Xp,Yp),(Xi,Yi)] – d (2.2.2.1.2-1)

Figure 2-17


2-22

Figure 2-18

so an equation can be written

(R – r)2 – (ρ1 – r)2 = (ρ3)2 (2.2.2.1.2-2)

that produces for r

r = [R2 – (ρ3)2 – (ρ1)2]/(2R – 2ρ1) (2.2.2.1.2-3)

2.2.2.2 Initial circle center and switch arc center belong to two half planes defined by the line L.

2.2.2.2.1 Line continues outside the circle (Figure 2-19)

In this case

ρ3 = ρ[(Xp,Yp),(Xi,Yi)] + d (2.2.2.2.1-1)

Equation for r

(ρ1 + r)2 = (R + r)2 – (ρ3)2 (2.2.2.2.1-2)

From (2.2.2.2.1-2) we have

r = [(ρ1)2 + (ρ3)2 – R2]/(2R – 2ρ1) (2.2.2.2.1-3)

2.2.2.2.2 Line continues inside the circle (Figure 2-20)

ρ3 = ρ[(Xp,Yp),(Xi,Yi)] – d (2.2.2.2.2-1)

Equation for r

(R – r)2 – (ρ3)2 = (ρ1 + r)2 (2.2.2.2.2-2)


2-23

that produces

r = [R2 – (ρ1)2 – (ρ3)2]/(2R + 2ρ1) (2.2.2.2.2-3)

Figure 2-19

Figure 2-20

Figure 2-21


2-24

2.2.2.3 Circle center (Xc,Yc) Є L1 (line L1 intersects the center of the circle)

2.2.2.3.1 Line goes outside of the circle (Figure 2.11).

An equation for r

(R + r)2 = (R + d)2 + r2 (2.2.2.3.1-1)

r = (2Rd + d2)/(2R) (2.2.2.3.1-2)

2.2.2.3.2 Line goes inside the circle (Figure 2.12)

(R – r)2 = (R – d)2 + r2 (2.2.2.3.2-1)

r = (2Rd – d2)/(2R) (2.2.2.3.2-2)

Figure 2-22

2.2.3 Line goes outside the circle

If the line goes outside of the circle, the switch arc radius is limited by the length of the line segment and the length of the circle arc segment. The connection between the switch arc radius and distance d between intersection point and a last point of the switch arc on the line L was considered in section 3.2. The maximum switch radius for the given line segment ΔL can be calculated by formulas (2.2.2.1.1-5) or (2.2.2.2.1-3) from 2.2.2.1.1 where d = ΔL. When considering the limitations imposed by the length of the circle arc (defined by the sweep angle and a circle radius), three cases are possible:


2-25

1. Circle init radius intersects with the line L continued in its positive direction (Figure 2-23);

2. Line L is parallel to the init radius (Figure 2-25);

3. Line L continued in the reverse direction intersects init radius (Figure 2-26) (by the init radius there a line segment that connects the circle arc center with the circle arc init point).

2.2.3.1 Line L and init radius continued in their positive intersecting directions

The init circle radius continuing in its positive direction intersects with the line L continued in its positive direction (Figure 2-23). It takes place if the circle arc init angle with the axis X positive direction α is less than the line L angle with the axis X positive direction φ and the circle arc sweep angle β < 0 (β < 0, α < φ) or in case: β > 0, α > φ.

Figure 2-23

Drop a perpendicular from the circle center Oc on the line L. Its length h can be calculated by (2.2.1.1-4). Now calculate the angle γ = arccos(h/R), where R – circle arc radius. The circle arc sweep angle θ so λ = θ + γ = θ + arccos(h/R). Now calculate the distance

d = ρ(Oc, P2) = h/cos(λ) (2.2.3.1-1)

To define the switch radius r, use a proportion

r/h = (d – R – r)/d (2.2.3.1-2)


2-26

or

rd = hd – hR – hr (2.2.3.1-3)

and for r the result is

r = h(d – R)/(d + h ) (2.2.3.1-4) Example 2-24



The circle is defined by its radius R = 50000, init angle α = 45o and sweep angle β= -45o and the init point (35355, 35355). The line L end point is (80000, 80000). The circle end angle = 0o, so the line init point coordinates (50000, 0). For the coefficients A, B, C in the line standard equation Ax + By + C = 0,

A = -(Y2 - Y1) = -0 + 80000 = 80000 , B = X2 - X1 = 80000 – 50000 = 30000,

C = X1(Y2 - Y1) - Y1(X2 - X1) = 50000(80000 – 0) – 0 = 4000000000. The length of h can be calculated as:

h = |C|/(A2 + B2)1/2 = 4000000000/(80000*80000 + 30000*30000)1/2 = 46816.5

The length h of the perpendicular can be also calculated by formulas (a.16),(a1.4) and (a.17) from Appendix1 (this approach is used in the sample program CircLine_ex_3_13a).

β = 45o = π/4 = 0.785398, γ = arccos(h/R) = arccos(46816.5/50000) = 0.358768.

λ = β + γ = 0.785398 + 0.358768 = 1.144166.

d = h/cos(λ) = 46816.5/cos(1.144166) = 113136 and the for the switch radius:

rmax = h(d – R)/(d + h) = 46816.5(113136-50000)/(113136 + 46816.5) = 18479


2-27

Figure 2-24

2.2.3.2 Line parallel to the circle arc init radius

a) Line direction coincides with the direction of the init radius

In this simple case (Figure 2-25) maximum switch radius is equal to the distance between line L and circle arc init radius. This distance is equal to length of perpendicular h dropped from the circle arc center Oc on the line L. To define it we can use the same formula (2.2.1.1-4).

Figure 2-2-25a

b) Line direction is opposite to the direction of the init radius


2-28

Figure 2-25b

Maximum switch radius is perpendicular to the line L at the line end point. h – the length of the perpendicular dropped from the circle center on the line. We can calculate an angle γ between the perpendicular at the line end point and the line that connect the centers of the init circle and a switch arc

γ = 180 – 90 – (180 – θ) = θ – 90 (2.2.3.2-1) To define max switch radius we can use an equation

(R + r)cosγ = r + h (2.2.3.2-1)

that produces

r = (Rcosγ – h)/(1 – cosγ) (2.2.3.2-1)

2.2.3.3 Line L and init radius continued in their reverse directions intersect

This case is shown in Figure 2-26.

1. Find the coordinates P1(X1,Y1) – intersection point of two lines init circle radius (Oc,P2) and a line L.

2. Knowing the coordinates of circle arc center Oc(Xc,Yc) and intersection point P1(X1,Y1) makes ρ1 = ρ(P1,Oc) = [(X1 - Xc)2 + (Y1 - Yc)2]1/2 known.


2-29

3. Know trajectory init point P2(X2,Y2), calculate ρ2 = ρ(p2, p1) =

[(X2 – X1)2 + (Y2 – Y1)2]1/2

4. Drop a perpendicular from the circle center Oc on the line L and calculate its length h by (2.2.1.1-4)

5. To get switch arc radius, use a proportion

ρ1/(ρ2 + r) = h/r or (2.2.3.3-1)

ρ1r = hρ2 + hr (2.2.3.3-2)

and finally the value for r is:

r = hρ2/(ρ1 - h) (2.2.3.3-3)

Figure 2-25

Example 2-27



The circle arc is defined by its radius R = 40000, init angle α = 30o and sweep angle β = -30o, init point (34641, 20000) – Figure 2-27.

Line L target point (130000, 25000).

Coordinates of the circle center Oc: Xc = 34641 – 40000cos(π/6) = 0, Yc = 20000 – 40000sin(π/6) = 0;

Define the coordinates of the intersection point P1(X1, Y1). For X1 and Y1, use expressions


2-30

By (a3.6)-(a3.7) from Appendix 3. q1 = ΔX1/ΔY1= (34641-0)/(20000-0) = 1.73205 Y1 = ((q1*Y1 + X21 – X11)*ΔY2 –Y21ΔX2)/(q1*ΔY2 – ΔX2) = ((1.73205*0 + 40000 –0)*(25000-0) - 0)/( 1.73205*(25000-0) - (130000-40000)) =

-21414X1 = X11 + q1 (Y1– Y11) = X1 = 0 + 1.73205(-21414- 0) = -37090

To get the length of the perpendicular h from the circle center on the line L, line L is needed.

Figure 2-26

standard equation in a form Ax + By + C = 0. For A,B,C we have

A = -(Y22 – Y21) = 25000, B = 130000 – 40000 = 90000, C =

C = X21(Y2 – Y21) - Y1(X22 – X21) = 40000(25000-0) = 109.

h = (0*25000 + 0*90000 + 109)/( 250002 + 900002)1/2 = 10706ρ1 = [(-37090)2 + (-21414)2]1/2 = 42828

ρ2 = [(-37090 – 34641)2 + -21414– 20000)2]1/2 =82828

and for maximum switch radius we get

r = hρ2/(ρ1 – h) = 10706*82828/(42828– 10706) = 27605

2.3 Circle – circle intersection

Note: C1 – first circle arc, C2 – second circle arc, Pi – point of two circle arcs intersection, Co1 – first circle center point, Co2 – second circle center point.


2-31

2.3.1 One of two circle arcs intersects the internal area of the second

If the circle arc C1 comes from inside of the circle C2 – Figure 2-28 (or circle C2 continues inside the circle C1), then the switch arc radius must satisfy the necessary condition

r ≤ (R2 + h)/2 (2.3.1-1)

where h = ρ(O2,P1) – distance from the circle C2 center O2 to the intersection point P1(X1,Y1) of the line O1O2 connecting the centers of two circles with the circle arc C1 (Figure 2-28). Condition (2.3.1-1) is not always sufficient– only in cases that the points of intersection O1O2 with C1 and C2 belong to C1 and C2: P1(X1,Y1) ∈ C1 and P2(X2.Y2) ∈ C2 (Figure 2-28).

Figure 2-28

In Figure 2-29 the case when a point of intersection of the line O1O2 does not belong to the circle arc C1 is presented. Line O1P1 goes through the circle C1 center O1 and its init point P1(X1,Y1). P2(X2,Y2) – intersection point of the line O1P1 with the circle arc C2 (calculation of the circle – line intersection point coordinates can be found in Appendix 4). By knowing the coordinates of two points P1 and P2, calculate the distance

d = ρ(P1,P2).

To define the maximum switch arc radius r, use the following system of equations:

(Xo – X1)/(X2 – X1) = r/d (2.3.1-2)

(Yo – Y1)/(Y2 – Y1) = r/d (2.3.1-3)


2-32

(Xo – Xc2)2 + (Yo – Yc2)2 = (R2 – r)2 (2.3.1-4)

From (4.1-2)

Xod – X1d = r(X2 – X1)

and for Xo

Xo = r[(X2 – X1)/d] + X1 = rC1 + X1 (2.3.1-5)

From (4.1-3)

Yod – Y1d = r(Y2 – Y1)

and for Yo

Yo = r[(Y2 – Y1)/d] + Y1 = rC2 + Y1 (2.3.1-6)

Substituting (4.5) and (4.6) into (4.4)

[rC1 + X1 – Xc2]2 + [rC2 + Y1 – Yc2]2 = (R2 – r)2 (2.3.1-7)

or

[rC1 + C3]2 + [rC2 + C4]2 = (R2 – r)2 (2.3.1-8)

(C1)2r2 + (2C1C3)r + (C3)2 + (C2)2r2 + (2C2C4)r + (C4)2 =

(R2)2– 2R2r + r2 (2.3.1-9)

Figure 2-29

r2[(C1)2 +(C2)2 – 1] + r[2C1C3+2C2C4 + 2R2] + (C3)2 +(C4)2–(R2)2 = 0 (2.3.1-10)

But when considering


2-33

(C1)2 + (C2)2 – 1 = [(X2 – X1)/d]2 + [(Y2 – Y1)/d]2 – 1 = d2/d2 – 1 = 0 (2.3.1-11)

as d2 = ρ2(P1,P2) = (X2 – X1)2 + (Y2 – Y1)2. and if noting

C6 = 2(C1C3 + C2C4 + R2), C7 = (C3)2 + (C4)2 – (R2)2

The results for r are as follows:

r = –C7/C6 (2.3.1-12) where

C1 = (X2 – X1)/d

C2 = (Y2 – Y1)/d

C3 = X1 – Xc2 (2.3.1-13)

C4 = Y1 – Yc2

C6 = 2(C1C3 + C2C4 + R2)

C7 = (C3)2 + (C4)2 – (R2)2

The maximum switch radius can be also estimated as a solution of the alternate system

(Xo – X1)/(Xo – Xc1) = r/(R1 + r) (2.3.1-14)

(Yo – Y1)/(Yo – Yc1) = r/(R1 + r) (2.3.1-15)

(Xo – Xc2)2 + (Yo – Yc2)2 = (R2 – r)2 (2.3.1-16)

Its advantage is that there is no need to calculate coordinates of the point P2.

From (2.3.1-14),

Xo = [r(X1 – Xc1) + X1R1]/R1 = [r(X1 – Xc1)/R1] + X1 = rC1 + X1 (2.3.1-17)

where C1 = (X1 – Xc1)/R1

and from (2.3.1-15)

Yo = [r(Y1 – Yc1) + Y1R1]/R1 = [r(Y1 – Yc1)/R1] + Y1 = rC1 + Y1 (2.3.1-18)

where C2 = (Y1 – Yc1)/R1

By substituting (2.3.1-17) and (2.3.1-18) with (2.3.1-16), the results are

[rC1 + X1 – Xc2]2 + [rC1 + Y1 – Yc2]2 = (R2 – r)2 (2.3.1-19)

or


2-34

(rC1 + C3)2 + (rC2 + C4)2 = (R2 – r)2 (2.3.1-20)

where

C3 = X1 – Xc2 and C4 = Y1 – Yc2

From (2.3.1-20),

r2C12 + 2C1C3r + C32 + r2C22 + 2C2C4r + C42 = R22 – 2R2r + r2 (2.3.1-21)

r2(C12 + C22 – 1) + r(2C1C3 + 2C2C4 + 2R2) + (C32 + C42 – R22) = 0 (2.3.1-22)

but

C12 + C22 = [(Xc1 – X1)2 + (Yc1 – Y1)2]/R12 = 1 (2.3.1-23) so we get

rC6 + C7 = 0 (2.3.1-24)

r = – C7/C6 (2.3.1-25) where

C1 = (X1 – Xc1)/R1

C2 = (Y1 – Yc1)/R1

C3 = X1 – Xc2

C4 = Y1 – Yc2

C6 = 2C1C3 + 2C2C4 + 2R2

C7 = C32 + C42 – R22 (2.3.1-26)

Example 2-30

(Motion Mathematic Lib Samples\Circle to Circle\ Section 2_3\

CircleCircle_Ex_2_30 – www.elmomc.com)

Circle C1 (Figure 2-30) is defined by its radius R1 = 40000, init point P1(-40000, 0), center coordinates (0,0), init 180o angle and a 90o sweep angle.

Circle C2 is defined by the radius R2 = 100000, init point (0, -40000) center coordinates (-65000, 35000) and -180o sweep angle.

Coordinates of point P1: X1 = -40000, Y1 = 0;

To get the coordinates of the point P2, use the circle C2 equation:

(X + 65000)2 + (Y – 35000)2 = 1000002

Know that Y2 = 0, for X2 use the following equation:


2-35

(X2 + 65000)2 + (– 35000)2 = 1000002 that produces X2 = -158675.

d = |X2 – X1| = 118675;

C1 = -1.0

C2 = 0

C3 = 25000

C4 = -35000

C6 = 150000

C7 = -8150000000

r = 54333.3333

Figure 2-30

Figure 2-31 shows when the line connecting circle C1 init point (X1,Y1) and its center point do not intersect circle C2. To define the maximum value of the switch radius r, use the following system of equations:

(Xo – X1)/(Xc1 – X1) = r/R1 (2.3.1-27)

(Yo – Y1)/(Yc1 – Y1) = r/R1 (2.3.1-28)

(Xo – Xc2)2 + (Yo – Yc2)2 = (r + R)2 (2.3.1-29)


2-36

From (2.3.1-27)

Figure 2-31

XoR1 – X1R1 = r(Xc1 – X1) (2.3.1-30)

or

Xo = X1 + r[(Xc1 – X1)/R1] = X1 + rC1 (2.3.1-31)

where C1 = (Xc1 – X1)/R1 (2.3.1-32)

From (2.3.1-28)

YoR1 – Y1R1 = r(Yc1 – Y1) (2.3.1-33)

or

Yo = Y1 + r[(Yc1 – Y1)/R1] = Y1 + rC2 (2.3.1-34)

where C2 = [(Yc1 – Y1)/R1 (2.3.1-35)

Substituting Xo and Yo into (2.3.1-29)

[X1 + rC1 – Xc2]2 + [Y1 + rC2 – Yc2]2 = (r + R2)2 (2.3.1-36)

If we note that X1 – Xc2 = C3 and Y1 – Yc2 = C4, then (2.3.1-37) can be written in a format

[rC1 + C3]2 + [rC2 + C4]2 = (r + R2)2

or


2-37

r2 C12 + (2C1C3)r + C32 + r2C22 + (2C2C4)r + C42 = r2 + (2R2)r + R22

(2.3.1-37)

or

r2(C12 + C22 – 1) + 2[C1C3 + C2C4 – R2]r + (C32 + C42 – R22) = 0 (2.3.1-38)

But

C12 + C22 = [(Xc1 – X1)2 + (Yc1 – Y1)2]/R12 = 1 (2.3.1-39)

So (4.1-38) transforms into

rC6 + C7 = 0 (2.3.1-40)

where

C6 = 2[C1C3 + C2C4 – R2] (2.3.1-41)

and

C7 = C32 + C42 – R22 (2.3.1-42)

So the an upper limit estimation for r is:

r = – C7/ C6 (2.3.1-29)

Another case when the line O1O2 does not intersect circle arc C2 is shown in the Figure 2-32. Draw a line O2P2 from the center of the circle C2 to its end point. If noticing that d = ρ(O2P2), the switch arc radius r is:

(X2 – Xo)/(X2 – Xc2) = r/d (2.3.1-30)

(Y2 – Yo)/(Y2 – Yc2) = r/d (2.3.1-31)

(Xo – Xc1)2 + (Yo – Yc1)2 = (R1 + r)2 (2.3.1-32)

From (2.3.1-30)

X2d – Xod = r(X2 – Xc2)

Xo = X2 – r(X2 – Xc2)/d = X2 + r(Xc2 – X2)/d = X2 + C1r (2.3.1-33)

From (2.3.1-31)

Y2d – Yod = r(Y2 – Yc2)

Yo = Y2 – r(Y2 – Yc2)/d = Y2 + r(Yc2 – Y2)/d = Y2 + C2r (2.3.1-34)


2-38

Substituting into (4.1-32)

(X2 + C1r – Xc1)2 + (Y2 + C2r – Yc1)2 = (R1 + r)2 (2.3.1-35)

(C1r + C3)2 + (C2r + C4)2 = (R1 + r)2 (2.3.1-36)

r2[(C1)2 +(C2)2 –1] + r[2C1C3+2C2C4 –2R1] + [(C3)2 + (C4)2 –(R1)2] = 0 (2.3.1-37)

But

(C1)2 +(C2)2 –1 = (Xc2 – X2)2/d2 + (Xc2 – X2)2/d2 –1 =

d2/d2 –1 = 0 (2.3.1-38)

So

r = –C7/C6 (2.3.1-39)

where

C1 = (Xc2 – X2)/d

C2 = (Yc2 – Y2)/d

C3 = X2 – Xc1 (2.3.1-40)

C4 = Y2 – Yc1

C6 = 2(C1C3 + C2C4 – R1)

C7 = (C3)2 + (C4)2 – (R1)2

Figure 2-32


2-39

2.3.2 Each circle intersects the internal area of the second

Figure 2-33 shows another case of two circles intersecting: circle C1 comes from inside of the circle C2 and circle C2 continues inside the circle C1. Noting that

d = ρ(O1, O2), the switch radius must obey the following necessary condition:

R ≤ [d – (d – R1) – (d – R2)]/2 = (R1 + R2 – d)/2 (2.3.2-1)

Condition (2.3.2-1) is sufficient in case line O1O2 intersects circle arc C1 and circle arc C2.

Figure 2-33

Line O1O2 does not intersect at least one of two circle segments (Figure 2-35)

Another case when line O1O2 does not intersect at least one of the two circle segments. In Figure 2-35 line (Xc1, Yc1) – (Xc2, Yc2) connecting two circle centers does not intersect circle arc C2. Draw a line O2P1 between the circle C2 center and its end point P1(X1,Y1). The center (Xo, Yo) of the switch arc with a maximum radius must belong to this line. To define maximum radius r, use the same system as in the previous cases.

(X1 – Xo)/(X1 – Xc2) = r/R2 (2.3.2-2)

(Y1 – Yo)/(Y1 – Yc2) = r/R2 (2.3.2-3)

(Xo – Xc1)2 + (Yo – Yc1)2 = (R1 – r)2 (2.3.2-4)


2-40

This system is similar to (2.3.2-2) – (2.3.2-4) and comes to the same solution r = –C7/C6 where

C1 = (X1 – Xc2)/R2

C2 = (Y1 – Yc2)/R2

C3 = Xc1 – X1 (2.3.2-5)

C4 = Yc1 – Y1

C6 = 2(C1C3 + C2C4 + R1)

C7 = (C3)2 + (C4)2 – (R1)2

Figure 2-35

Example 2-36

(Motion Mathematic Lib Samples\Circle to Circle\ Section 2_3\


Consider an example (Figure 2-36) of a maximum switch radius calculation for the case similar to the one drawn in Figure 2-35 with the following parameters:

Coordinates of the first arc center are Xc1 = 0, Yc1 = 0. Coordinates of the second arc center are Xc1 = 100000, Yc1 = 100000.

Circle arc C1 init angle α1 = 0o, sweep angle β1 = 90o, radius R1 = 100000. Circle arc C2 init angle α2 = 180o, sweep angle β2 = 30o, radius R2 = 100000. Calculations due to (4.2-5):

X1 = 100000 – 100000*cos(pi/6) = 13397.459622

Y1 = 100000 – 100000*sin(pi/6) = 50000


2-41

C1 = (X1 – Xc2)/R2 = -0.866025

C2 = (Y1 – Yc2)/R2 = -0.5

C3 = Xc1 – X1 = -13397.459622

C4 = Yc1 – Y1 = -50000

C6 = 2(C1C3 + C2C4 + R1) = 273205.080767

C7 = (C3)2 + (C4)2 – (R1)2 = –7320508075.6888

r = -C7/C6 = 26794.919242

The switch radius in use must be less this value.

Figure 2-36

Line O1O2 does not intersect at least one of the circle segments (Figure 2-37)

Another case when the line O1O2 does not intersect at least one of the circle segments. In Figure 2-37 line (Xc1, Yc1) – (Xc2, Yc2) connecting two circle centers does not intersect circle arc C1. Draw a line O1P1 between the circle C1 center and its start point P1(X1,Y1). Center (Xo, Yo) of the switch arc with maximum radius must belong to this line. To define maximum radius r, use the same system as in previous cases:

(X1 – Xo)/(X1 – Xc1) = r/R1 (2.3.2-6)

(Y1 – Yo)/(Y1 – Yc1) = r/R1 (2.3.2-7)

(Xo – Xc2)2 + (Yo – Yc2)2 = (R2 – r)2 (2.3.2-8)


2-42

This system is similar to (4.2) – (4.4) and comes to the same solution r = –C7/C6 where

Figure 2-37

C1 = (X1 – Xc2)/R1

C2 = (Y1 – Yc2)/R1

C3 = Xc2 – X1 (2.3.2--9)

C4 = Yc2 – Y1

C6 = 2(C1C3 + C2C4 + R2)

C7 = (C3)2 + (C4)2 – (R2)2

2.3.3 No circle intersects the internal area of the other

In case the circle C2 goes outside circle C1 and circle C1 doesn’t come from the internal area of the circle C2, restrictions on the switch radius are still possible if a sweep angle of at least one circle is less than 90o.


2-43

Consider the case that the sweep angle of the first circle is β1 < 90 and the sweep angle of

the second circle is β2 < 90. Draw a line L1 defined by two points: circle C1 center (Xc1,

Yc1) and circle C1 start point (X1,Y1) and line L2 defined by circle C2 center point (Xc2,

Yc2) and circle C2 end point. If line L1 does not intersect circle arc C2 and line L2 does not

intersect the circle arc C1, then the intersection point of two lines is point (X3, Y3) – Figure

4.3. Note: l1 the length of the line L1 : l1 = ρ((X1,Y1),(X3,Y3)) and l2 the length of the

line L2: l2 = ρ((X2,Y2),(X3,Y3)).

If l1 >l2 for the maximum switch radius r calculation, use the following system:

(Xo – X1)/(X1 – Xc1) = r/R1

(2.3.3-1)

(Yo – Y1)/(Y1 – Yc1) = r/R1

(2.3.3-2)

(Xo – Xc2)2 + (Yo –Yc2)2 = (R2 + r) (2.3.3-3)

Equations (2.3.3-1)-(2.3.3-2) can be written in the following format

Xo = X1 + r(X1 – Xc1)/R1 = X1 + rC1 , C1 = (X1 – Xc1)/R1

(2.3.3-4)

Yo = Y1 + r(Y1 – Yc1)/R1 = Y1 + rC2 , C2 = (Y1 – Yc1)/R1

(2.3.3-5)

Substituting into (2.3.3-3), the results are:

(X1 + rC1 – Xc2 )2 + (Y1 + rC2 – Yc2)2 = (rC1 + C3)2 + (rC2 + C4)2 = (R2 + r)2

(2.3.3-6)

where C3 = X1 – Xc2 C4 = Y1 – Yc2.

Simplifying (2.3.3-6), the results are:

r2[1 – (C1)2 – (C2)2] + r[2R2 – 2C1C3 – 2C2C4] +[(R2)2 – (C3)2 – (C4)2] =


2-44

r2C5 + rC6 + C7 = 0 (2.3.3-7)

But

1 – (C1)2 – (C2)2 = 1 – (X1 – Xc1)2/(R1)2 – (Y1 – Yc1)2/(R1)2 = 0 (2.3.3-8)

So for r the results are:

r = – C7/C6 (2.3.3-9)

Similarly in the case when l2 > l1, use the following system:

(Xo – X2)/(X2 – Xc2) = r/R2 (2.3.3-10)

(Yo – Y2)/(Y2 – Yc2) = r/R2 (2.3.3-11)

(Xo – Xc1)2 + (Yo –Yc1)2 = (R1 + r) (2.3.3-12)

Equations (2.3.3-1) - (2.3.3-2) can be written in the following format

Xo = X2 + r(X2 – Xc2)/R2 = X2 + rC1 , C1 = (X2 – Xc2)/R2 (2.3.3-13)

Yo = Y2 + r(Y2 – Yc2)/R2 = Y2 + rC2 , C2 = (Y2 – Yc2)/R2 (2.3.3-14)

And substituting into (2.3.3-3), the results are:

(X2 + rC1 – Xc1 )2 + (Y2 + rC2 – Yc1)2 = (rC1 + C3)2 + (rC2 + C4)2 = (R2 + r)2

(2.3.3-15)

where C3 = X2 – Xc1 C4 = Y2 – Yc1.

Simplifying (2.3.3-15), the results are:

r2[1 – (C1)2 – (C2)2] + r[2R2 – 2C1C3 – 2C2C4] +[(R2)2 – (C3)2 – (C4)2] =

r2C5 + rC6 + C7 = 0 (2.3.3-16)

But

1 – (C1)2 – (C2)2 = 1 – (X2– Xc2)2/(R2)2 – (Y2 – Yc2)2/(R2)2 = 0 (2.3.3-17)


2-45

So for r, the results are:

r = –C7/C6 (2.3.3-18)

If L1 intersects circle arc C2, use point (X1,Y1). If L2

intersects circle arc C1, use point

(X1,Y1).

Figure 2-38

Appendix A: Projection of a point on a line defined by the end points

The line L is defined by its end points P1(X1,Y1) and P2(X2,Y2). Drop a perpendicular from some point (Xo,Yo) on the line L and find coordinates (Xpr, Ypr) of the projection point and the length of the perpendicular h.

a) X1 = X2

Xpr = X1

Ypr = Yo h = |Xo – X1| b) Y2 = Y1

Xpr = Xo

Ypr = Y1 h= |Yo – Y1| c) X1 ≠ X2 and Y2 ≠ Y1

Line equation

(X– X1)/(X2 – X1) = (Y – Y1)/(Y2 – Y1) (a1.1)

That can be transformed into

(Y – Y1)∆X = (X – X1)∆Y or

Y = (Y1dX + X∆Y – X1∆Y)/∆X = Y1 + k(X – X1) (a1.2)

Where k = ∆Y/∆X = (Y2 – Y1)/(X2 – X1) An equation of the perpendicular dropped from (Xo,Yo) on L can be written in a form

(Y - Yo) = (-1/k)(X – Xo) = q(X – Xo) (a1.3)

or

Y = Yo + q(X – Xo) (a1.4)

Substituting (a1.4) into (a1.2) the results are

Yo + q(X – Xo) = Y1 + k(X – X1) (a1.5)

That produces for X coordinate

X = (Yo – Y1 + kX1 – qXo)/(k – q) (a1.6)

Maestro Software Manual MAN-MLT(Ver. 2.0)

A-1

Y is from (a1.4). Coordinates (X,Y) of the intersection point line L and perpendicular are coordinates of projection point (Xp,Yp).

Having got a projection point (Xp,Yp) we can calculate the perpendicular length h as Euclidian distance

h = [(Xp – Xo)2 + (Yp – Yo)2]1/2 (a1.7)

Maestro Software Manual Setting up the Demo Case MAN-MLT(Ver. 2.0)

A-2

Appendix B: Coefficients of the line standard equation for the line defined by the end points If the line L is defined by its end points (X1,Y1) and (X2,Y2) we can calculate the coefficients A,B,C for the three possible cases:

a) X1 ≠ X2 and Y1 ≠ Y2 .

The line equation can be written in a form

(X – X1)/(X2 – X1) = (Y – Y1)/(Y2 – Y1) (a2.1)

or

(X – X1)/ ∆X = (Y – Y1)/∆Y (a2.2)

that can be transformed into

X∆Y – X1∆Y –Y∆X + Y1∆X = X∆Y –Y∆X +(Y1∆X - X1∆Y) = 0 (a2.3)

From (a2.3) we get

A = ∆Y, B = –∆X, C = Y1∆X – X1∆Y (a2.4)

b) X1 ≠ X2 and Y1 = Y2 .

The line equation is

Y = Y1 or Y – Y1 = 0 (a2.5)

So for A, B and C we have

A = 0, B = 1, C = –Y1 (a2.6)

c) X1 = X2 and Y1 ≠ Y2 . The line equation is

X = X1 or X – X1 = 0 (a2.7)

So for A, B, C we have

A = 1, B = 0, C = – X1 (a2.8)

Maestro Software Manual MAN-MLT (Ver. 2.0)

B-1

Appendix C: Intersection point of two lines defined by the end points

Line L1 is defined by its end points P1(X1,Y1) and P2(X2,Y2). Line L2 is defined by end points P3(X3,Y3) and P4(X4,Y4).

Note: ∆X1 = X2 – X1, ∆Y1 = Y2 – Y1, ∆X2 = X4 – X3, ∆Y2 = Y4 – Y3,

k1 = ∆Y1/∆X1, q1 = ∆X1/∆Y1, k2 = ∆Y2/∆X2, q2 = ∆X2/∆Y2

Calculation of the two lines intersection point (X,Y) depends on the each line position relative to coordinate axes. The following cases are possible.

a) X1 ≠ X2, Y1 ≠ Y2, X3 ≠ X4, Y3 ≠ Y4

To define intersection point, use

(X – X1)∆Y1 = (Y – Y1)∆X1 (a3.1)

(X – X3)∆Y2 = (Y – Y3)∆X2 (a3.2)

or

X∆Y1 – X1∆Y1 = Y∆X1 – Y1∆X1 (a3.3)

X∆Y2 – X3∆Y2 = Y∆X2 – Y3∆X2 (a3.4)

From (a3.3) the results are

X = X1 + Yq1 – Y1q1 (a3.5)

and substituting into (a3.4) the results are

(X1 + Yq1 – Y1q1)∆Y2 – X3∆Y2 = Y∆X2 – Y3∆X2 (a3.6)

that produces

Y = [(q1Y1 + X3 – X1)∆Y2 –Y3∆X2]/(q1∆Y2 – ∆X2) (a3.7)

and X can be calculated by (a3.5).

b) X1 ≠ X2, Y1 ≠ Y2, X3 = X4, Y3 ≠ Y4

To find an intersection point use

(X – X1)/∆X1 = (Y – Y1)/∆Y1 (a3.8)

X = X3 (a3.9)


C-1

or

(X3 – X1)/∆X1 = (Y – Y1)/∆Y1 (a3.10) and finally for coordinate Y of intersection point the results are

Y = Y1 + k1(X3 – X1) (a3.11)

c) X1 ≠ X2, Y1 ≠ Y2, X3 ≠ X4, Y3 = Y4

To find an intersection point use

(X – X1)/∆X1 = (Y – Y1)/∆Y1 (a3.12)

Y = Y3 (a3.13)

That produces

X = X1 + q1(Y3 – Y1) (a3.14)

d) X1 = X2, Y1 ≠ Y2, X3 ≠ X4, Y3 ≠ Y4

Two lines equations:

X = X1 (a3.15)

(X – X3)/∆X2 = (Y – Y3)/∆Y2 (a3.16)

For coordinate Y of intersection point we have

(X1 – X3)/∆X2 = (Y – Y3)/∆Y2 => Y = Y3 + k2(X1 – X3) (a3.17)

e) X1 ≠ X2, Y1 = Y2, X3 ≠ X4, Y3 ≠ Y4

Two lines equations:

Y = Y1

(X – X3)/∆X2 = (Y – Y3)/∆Y2

For coordinate X of intersection point the results are

(X – X3)/∆X2 = (Y1 – Y3)/∆Y2 => X = X3 + q2(Y1 – Y3) (a3.18)

f) X1 = X2, Y1 ≠ Y2, X3 ≠ X4, Y3 =Y4

X = X1, Y = Y3

g) X1 ≠ X2, Y1 = Y2, X3 = X4, Y3 ≠Y4

X = X3, Y = Y1

Maestro Software Manual Performance Considerations MAN-MLT (Ver. 2.0)

C-2

Appendix D: Circle – line intersection points

The line is defined by its end points (X1,Y1) and (X2,Y2). The circle is defined by its radius R and center coordinates (Xo, Yo). Consider the general case X1 ≠ X2 and Y1 ≠ Y2. In this case, calculate the intersection points using

(X – X1)/(X2 – X1) = (Y – Y1)/(Y2 – Y1) (a4.1) (X – Xo)2 + (Y – Yo)2 = R2 (a4.2) Note that k = (Y2 – Y1)/(X2 – X1) and C1 = Y1 – kX1 equation (a4.1) can be written in the following form

Y = kX + (Y1 – kX1) = kX+ C1 (a4.3) Substituting (a4.3) into (a4.2) results in

(X – Xo)2 + (kX + C1 – Yo)2 – R2 = 0 (a4.4) Simplifying (a4.4) results in the following equation

C3X2 + C4X + C5 = 0 (a4.5) where

C2 = C1 – Yo, C3 = 1 + k2, C4 = 2*(–Xo + kC2), C5 = (Xo)2 + (C2)2 – R2

Note that d = (C4)2 – 4C3C5 and for intersection point X coordinates the results are

X1 = (–C4 + d1/2)/(2C3) (a4.6) X2 = (–C4 – d1/2)/(2C3) (a4.7) Respective Y coordinates can be calculated by (a4.3) as

Y1 = kX1 + C1 (a4.8) Y2 = kX2 + C1 (a4.9) Consider the case X1 = X2. In this case line equation

X = X1

and substituting into (a4.2) the results are

(Y – Yo)2 = R2 – (X1 – Xo)2 (a4.10) and for Y the results are

Y 1= Yo + [R2 – (X1 – Xo)2]1/2 (a4.11) Y 2= Yo – [R2 – (X1 – Xo)2]1/2 (a4.12)


D-1

Documents

Motion Control Library · Chapter 1: General Description 1.1 Introduction The Motion Library (ML) produces trajectories based on the PVT mechanism. It implements a set of functions