1
# ***************************************************************************
2
# * Copyright (c) 2009, 2010 Yorik van Havre <yorik@uncreated.net> *
3
# * Copyright (c) 2009, 2010 Ken Cline <cline@frii.com> *
5
# * This program is free software; you can redistribute it and/or modify *
6
# * it under the terms of the GNU Lesser General Public License (LGPL) *
7
# * as published by the Free Software Foundation; either version 2 of *
8
# * the License, or (at your option) any later version. *
9
# * for detail see the LICENCE text file. *
11
# * This program is distributed in the hope that it will be useful, *
12
# * but WITHOUT ANY WARRANTY; without even the implied warranty of *
13
# * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
14
# * GNU Library General Public License for more details. *
16
# * You should have received a copy of the GNU Library General Public *
17
# * License along with this program; if not, write to the Free Software *
18
# * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 *
21
# ***************************************************************************
22
"""Provide vector math utilities used in the Draft workbench.
24
Vector math utilities used primarily in the Draft workbench
25
but which can also be used in other workbenches and in macros.
27
## \defgroup DRAFTVECUTILS DraftVecUtils
29
# \brief Vector math utilities used in Draft workbench
31
# Vector math utilities used primarily in the Draft workbench
32
# but which can also be used in other workbenches and in macros.
35
# flake8 --ignore=E226,E266,E401,W503
40
from FreeCAD import Vector
41
from draftutils import params
42
from draftutils import messages
44
__title__ = "FreeCAD Draft Workbench - Vector library"
45
__author__ = "Yorik van Havre, Werner Mayer, Martin Burbaum, Ken Cline"
46
__url__ = "https://www.freecad.org"
48
## \addtogroup DRAFTVECUTILS
53
"""Get the number of decimal numbers used for precision.
58
Return the number of decimal places set up in the preferences,
59
or a standard value (6), if the parameter is missing.
61
return params.get_param("precision")64
def typecheck(args_and_types, name="?"):
65
"""Check that the arguments are instances of certain types.
70
A list of tuples. The first element of a tuple is tested as being
71
an instance of the second element.
73
args_and_types = [(a, Type), (b, Type2), ...]
80
A `Type` can also be a tuple of many types, in which case
81
the check is done for any of them.
83
args_and_types = [(a, (Type3, int, float)), ...]
85
isinstance(a, (Type3, int, float))
88
Defaults to `'?'`. The name of the check.
93
If the first element in the tuple is not an instance of the second
96
for v, t in args_and_types:
97
if not isinstance(v, t):
98
messages._wrn("typecheck[{0}]: {1} is not {2}".format(name, v, t))99
raise TypeError("fcvec." + str(name))103
"""Return a string with the Python command to recreate this vector.
107
u : list, or Base::Vector3
108
A list of FreeCAD.Vectors, or a single vector.
113
The string with the code that can be used in the Python console
114
to create the same list of vectors, or single vector.
116
if isinstance(u, list):
120
s += "FreeCAD.Vector("121
s += str(v.x) + ", " + str(v.y) + ", " + str(v.z)
123
# This test isn't needed, because `first` never changes value?
127
# Remove the last comma
132
s = "FreeCAD.Vector("133
s += str(u.x) + ", " + str(u.y) + ", " + str(u.z)
138
def tup(u, array=False):
139
"""Return a tuple or a list with the coordinates of a vector.
145
array : bool, optional
146
Defaults to `False`, and the output is a tuple.
147
If `True` the output is a list.
152
The coordinates of the vector in a tuple `(x, y, z)`
153
or in a list `[x, y, z]`, if `array=True`.
155
typecheck([(u, Vector)], "tup")
157
return [u.x, u.y, u.z]
159
return (u.x, u.y, u.z)
163
"""Return the negative of a given vector.
173
A vector in which each element has the opposite sign of
174
the original element.
176
typecheck([(u, Vector)], "neg")
177
return Vector(-u.x, -u.y, -u.z)
181
"""Check for equality between two vectors.
183
Due to rounding errors, two vectors will rarely be `equal`.
184
Therefore, this function checks that the corresponding elements
185
of the two vectors differ by less than the decimal `precision` established
186
in the parameter database, accessed through `FreeCAD.ParamGet()`.
202
`True` if the vectors are within the precision, `False` otherwise.
204
typecheck([(u, Vector), (v, Vector)], "equals")
205
return isNull(u.sub(v))
209
"""Scales (multiplies) a vector by a scalar factor.
214
The FreeCAD.Vector to scale.
221
The new vector with each of its elements multiplied by `scalar`.
223
typecheck([(u, Vector), (scalar, (int, int, float))], "scale")
224
return Vector(u.x*scalar, u.y*scalar, u.z*scalar)
228
"""Scale a vector so that its magnitude is equal to a given length.
230
The magnitude of a vector is
232
L = sqrt(x**2 + y**2 + z**2)
234
This function multiplies each coordinate, `x`, `y`, `z`,
235
by a factor to produce the desired magnitude `L`.
236
This factor is the ratio of the new magnitude to the old magnitude,
238
x_scaled = x * (L_new/L_old)
245
The new magnitude of the vector in standard units (mm).
250
The new vector with each of its elements scaled by a factor.
251
Or the same input vector `u`, if it is `(0, 0, 0)`.
253
typecheck([(u, Vector), (l, (int, int, float))], "scaleTo")
258
return Vector(u.x*a, u.y*a, u.z*a)
262
"""Return the distance between two points (or vectors).
267
First point, defined by a vector.
269
Second point, defined by a vector.
274
The scalar distance from one point to the other.
276
typecheck([(u, Vector), (v, Vector)], "dist")
277
return u.sub(v).Length
280
def angle(u, v=Vector(1, 0, 0), normal=Vector(0, 0, 1)):
281
"""Return the angle in radians between the two vectors.
283
It uses the definition of the dot product
285
A * B = |A||B| cos(angle)
287
If only one vector is given, the angle is between that one and the
290
If a third vector is given, it is the normal used to determine
291
the sign of the angle.
292
This normal is used to calculate a `factor` as the dot product
293
with the cross product of the first two vectors.
298
If the `factor` is positive the angle is positive, otherwise
299
it is the opposite sign.
305
v : Base::Vector3, optional
306
The second vector to test against the first one.
307
It defaults to `(1, 0, 0)`, or +X.
308
normal : Base::Vector3, optional
309
The vector indicating the normal.
310
It defaults to `(0, 0, 1)`, or +Z.
315
The angle in radians between the vectors.
316
It is zero if the magnitude of one of the vectors is zero,
317
or if they are colinear.
319
typecheck([(u, Vector), (v, Vector)], "angle")
320
ll = u.Length * v.Length
324
# The dot product indicates the projection of one vector over the other
327
# Due to rounding errors, the dot product could be outside
328
# the range [-1, 1], so let's force it to be within this range.
336
# The cross product compared with the provided normal
338
coeff = normal.dot(normal1)
346
"""Project the first vector onto the second one.
348
The projection is just the second vector scaled by a factor.
349
This factor is the dot product divided by the square
350
of the second vector's magnitude.
352
f = A * B / |B|**2 = |A||B| cos(angle) / |B|**2
353
f = |A| cos(angle)/|B|
365
The new vector, which is the same vector `v` scaled by a factor.
366
Return `Vector(0, 0, 0)`, if the magnitude of the second vector
369
typecheck([(u, Vector), (v, Vector)], "project")
371
# Dot product with itself equals the magnitude squared.
374
return Vector(0, 0, 0) # to avoid division by zero
375
# Why specifically this value? This should be an else?
377
return scale(v, u.dot(v)/dp)
379
# Return a null vector if the magnitude squared is 15, why?
380
return Vector(0, 0, 0)
383
def rotate2D(u, angle):
384
"""Rotate the given vector around the Z axis by the specified angle.
386
The rotation occurs in two dimensions only by means of
390
(x_rot) = (cos(-angle) -sin(-angle)) * (x)
391
(y_rot) (sin(-angle) cos(-angle)) (y)
393
Normally the angle is positive, but in this case it is negative.
395
`"Such non-standard orientations are rarely used in mathematics
396
but are common in 2D computer graphics, which often have the origin
397
in the top left corner and the y-axis pointing down."`
398
W3C Recommendations (2003), Scalable Vector Graphics: the initial
406
The angle of rotation given in radians.
411
The new rotated vector.
413
x_rot = math.cos(-angle) * u.x - math.sin(-angle) * u.y
414
y_rot = math.sin(-angle) * u.x + math.cos(-angle) * u.y
416
return Vector(x_rot, y_rot, u.z)
419
def rotate(u, angle, axis=Vector(0, 0, 1)):
420
"""Rotate the vector by the specified angle, around the given axis.
422
If the axis is omitted, the rotation is made around the Z axis
425
It uses a 3x3 rotation matrix.
429
(c + x*x*t xyt - zs xzt + ys )
430
u_rot = (xyt + zs c + y*y*t yzt - xs ) * u
431
(xzt - ys yzt + xs c + z*z*t)
433
Where `x`, `y`, `z` indicate unit components of the axis;
434
`c` denotes a cosine of the angle; `t` indicates a complement
435
of that cosine; `xs`, `ys`, `zs` indicate products of the unit
436
components and the sine of the angle; and `xyt`, `xzt`, `yzt`
437
indicate products of two unit components and the complement
445
The angle of rotation given in radians.
446
axis : Base::Vector3, optional
447
The vector specifying the axis of rotation.
448
It defaults to `(0, 0, 1)`, the +Z axis.
453
The new rotated vector.
454
If the `angle` is zero, return the original vector `u`.
456
typecheck([(u, Vector), (angle, (int, float)), (axis, Vector)], "rotate")
461
# Unit components, so that x**2 + y**2 + z**2 = 1
479
m = FreeCAD.Matrix(c + x*x*t, xyt - zs, xzt + ys, 0,
480
xyt + zs, c + y*y*t, yzt - xs, 0,
481
xzt - ys, yzt + xs, c + z*z*t, 0)
486
def getRotation(vector, reference=Vector(1, 0, 0)):
487
"""Return a quaternion rotation between a vector and a reference.
489
If the reference is omitted, the +X axis is used.
493
vector : Base::Vector3
495
reference : Base::Vector3, optional
496
The reference vector. It defaults to `(1, 0, 0)`, the +X axis.
501
A tuple with the unit elements (normalized) of the cross product
502
between the `vector` and the `reference`, and a `Q` value,
503
which is the sum of the products of the magnitudes,
504
and of the dot product of those vectors.
506
Q = |A||B| + |A||B| cos(angle)
508
It returns `(0, 0, 0, 1.0)`
509
if the cross product between the `vector` and the `reference`
516
c = vector.cross(reference)
518
return (0, 0, 0, 1.0)
521
q1 = math.sqrt((vector.Length**2) * (reference.Length**2))
522
q2 = vector.dot(reference)
525
return (c.x, c.y, c.z, Q)
529
"""Return False if each of the components of the vector is zero.
531
Due to rounding errors, an element is probably never going to be
532
exactly zero. Therefore, it rounds the element by the number
533
of decimals specified in the `precision` parameter
534
in the parameter database, accessed through `FreeCAD.ParamGet()`.
535
It then compares the rounded numbers against zero.
539
vector : Base::Vector3
545
`True` if each of the elements is zero within the precision.
549
x = round(vector.x, p)
550
y = round(vector.y, p)
551
z = round(vector.z, p)
552
return (x == 0 and y == 0 and z == 0)
555
def find(vector, vlist):
556
"""Find a vector in a list of vectors, and return the index.
558
Finding a vector tests for `equality` which depends on the `precision`
559
parameter in the parameter database.
563
vector : Base::Vector3
566
A list of Base::Vector3 vectors.
571
The index of the list where the vector is found,
572
or `None` if the vector is not found.
576
equals : test for equality between two vectors
578
typecheck([(vector, Vector), (vlist, list)], "find")
579
for i, v in enumerate(vlist):
580
if equals(vector, v):
585
def closest(vector, vlist, return_length=False):
586
"""Find the closest point to one point in a list of points (vectors).
588
The scalar distance between the original point and one point in the list
589
is calculated. If the distance is smaller than a previously calculated
590
value, its index is saved, otherwise the next point in the list is tested.
594
vector: Base::Vector3
595
The tested point or vector.
598
A list of points or vectors.
600
return_length: bool, optional
601
It defaults to `False`.
602
If it is `True`, the value of the smallest distance will be returned.
607
The index of the list where the closest point is found.
610
If `return_length` is `True`, it returns both the index
611
and the length to the closest point.
613
typecheck([(vector, Vector), (vlist, list)], "closest")
615
# Initially test against a very large distance, then test the next point
616
# in the list which will probably be much smaller.
617
dist = 9999999999999999
619
for i, v in enumerate(vlist):
620
d = (vector - v).Length
631
def isColinear(vlist):
632
"""Check if the vectors in the list are colinear.
634
Colinear vectors are those whose angle between them is zero.
636
This function tests for colinearity between the difference
637
of the first two vectors, and the difference of the nth vector with
640
vlist = [a, b, c, d, ..., n]
656
List of Base::Vector3 vectors.
657
At least three elements must be present.
662
`True` if the vector differences are colinear,
663
or if the list only has two vectors.
668
Due to rounding errors, the angle may not be exactly zero;
669
therefore, it rounds the angle by the number
670
of decimals specified in the `precision` parameter
671
in the parameter database, and then compares the value to zero.
673
typecheck([(vlist, list)], "isColinear")
675
# Return True if the list only has two vectors, why?
676
# This doesn't test for colinearity between the first two vectors.
682
# Difference between the second vector and the first one
683
first = vlist[1].sub(vlist[0])
685
# Start testing from the third vector and onward
686
for i in range(2, len(vlist)):
688
# Difference between the 3rd vector and onward, and the first one.
689
diff = vlist[i].sub(vlist[0])
691
# The angle between the difference and the first difference.
692
_angle = angle(diff, first)
694
if round(_angle, p) != 0:
699
def rounded(v,d=None):
700
"""Return a vector rounded to the `precision` in the parameter database
701
or to the given decimals value
703
Each of the components of the vector is rounded to the decimal
704
precision set in the parameter database.
710
d : (Optional) the number of decimals to round to
715
The new vector where each element `x`, `y`, `z` has been rounded
716
to the number of decimals specified in the `precision` parameter
717
in the parameter database.
722
return Vector(round(v.x, p), round(v.y, p), round(v.z, p))
725
def getPlaneRotation(u, v, _ = None):
726
"""Return a rotation matrix defining the (u,v,w) coordinate system.
728
The rotation matrix uses the elements from each vector.
729
`v` is adjusted to be perpendicular to `u`
741
Hint for the second vector.
742
_ : Ignored. For backwards compatibility
747
The new rotation matrix defining a new coordinate system,
748
or `None` if `u` or `v` is `None` or
749
if `u` and `v` are parallel.
751
if (not u) or (not v):
753
typecheck([(u, Vector), (v, Vector)], "getPlaneRotation")
762
m = FreeCAD.Matrix(u.x, v.x, w.x, 0,
769
def removeDoubles(vlist):
770
"""Remove duplicated vectors from a list of vectors.
772
It removes only the duplicates that are next to each other in the list.
774
It tests the `i` element, and compares it to the `i+1` element.
775
If the former one is different from the latter,
776
the former is added to the new list, otherwise it is skipped.
777
The last element is always included.
779
[a, b, b, c, c] -> [a, b, c]
780
[a, a, b, a, a, b] -> [a, b, a, b]
782
Finding duplicated vectors tests for `equality` which depends
783
on the `precision` parameter in the parameter database.
787
vlist : list of Base::Vector3
792
list of Base::Vector3
793
New list with sequential duplicates removed,
794
or the original `vlist` if there is only one element in the list.
798
equals : test for equality between two vectors
800
typecheck([(vlist, list)], "removeDoubles")
805
# Iterate until the penultimate element, and test for equality
806
# with the element in front
807
for i in range(len(vlist) - 1):
808
if not equals(vlist[i], vlist[i+1]):
809
nlist.append(vlist[i])
810
# Add the last element
811
nlist.append(vlist[-1])
814
def get_spherical_coords(x, y, z):
815
"""Get the Spherical coordinates of the vector represented
816
by Cartesian coordinates (x, y, z).
820
vector : Base::Vector3
826
Tuple (radius, theta, phi) with the Spherical coordinates.
827
Radius is the radial coordinate, theta the polar angle and
828
phi the azimuthal angle in radians.
832
The vector (0, 0, 0) has undefined values for theta and phi, while
833
points on the z axis has undefined value for phi. The following
834
conventions are used (useful in DraftToolBar methods):
835
(0, 0, 0) -> (0, pi/2, 0)
836
(0, 0, z) -> (radius, theta, 0)
840
x_axis = Vector(1,0,0)
841
z_axis = Vector(0,0,1)
842
y_axis = Vector(0,1,0)
845
if not bool(round(rad, precision())):
846
return (0, math.pi/2, 0)
848
theta = v.getAngle(z_axis)
849
v.projectToPlane(Vector(0,0,0), z_axis)
850
phi = v.getAngle(x_axis)
852
return (rad, theta, 0)
853
# projected vector is on 3rd or 4th quadrant
854
if v.dot(Vector(y_axis)) < 0:
857
return (rad, theta, phi)
860
def get_cartesian_coords(radius, theta, phi):
861
"""Get the three-dimensional Cartesian coordinates of the vector
862
represented by Spherical coordinates (radius, theta, phi).
867
Radial coordinate of the vector.
869
Polar coordinate of the vector in radians.
871
Azimuthal coordinate of the vector in radians.
876
Tuple (x, y, z) with the Cartesian coordinates.
879
x = radius*math.sin(theta)*math.cos(phi)
880
y = radius*math.sin(theta)*math.sin(phi)
881
z = radius*math.cos(theta)