FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2005 Imetric 3D GmbH                                    *
3
 *                                                                         *
4
 *   This file is part of the FreeCAD CAx development system.              *
5
 *                                                                         *
6
 *   This library is free software; you can redistribute it and/or         *
7
 *   modify it under the terms of the GNU Library General Public           *
8
 *   License as published by the Free Software Foundation; either          *
9
 *   version 2 of the License, or (at your option) any later version.      *
10
 *                                                                         *
11
 *   This library  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.                  *
15
 *                                                                         *
16
 *   You should have received a copy of the GNU Library General Public     *
17
 *   License along with this library; see the file COPYING.LIB. If not,    *
18
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
19
 *   Suite 330, Boston, MA  02111-1307, USA                                *
20
 *                                                                         *
21
 ***************************************************************************/
22

23

24
#include "PreCompiled.h"
25

26
#include <cmath>
27
#include <limits>
28
#include <boost/math/special_functions/fpclassify.hpp>
29

30
#include "Vector3D.h"
31
#include "Tools.h"
32

33

34
using namespace Base;
35

36
template<class float_type>
37
Vector3<float_type>::Vector3(float_type fx, float_type fy, float_type fz)
38
    : x(fx)
39
    , y(fy)
40
    , z(fz)
41
{}
42

43
template<class float_type>
44
float_type& Vector3<float_type>::operator[](unsigned short usIndex)
45
{
46
    switch (usIndex) {
47
        case 0:
48
            return x;
49
        case 1:
50
            return y;
51
        case 2:
52
            return z;
53
    }
54
    return x;
55
}
56

57
template<class float_type>
58
const float_type& Vector3<float_type>::operator[](unsigned short usIndex) const
59
{
60
    switch (usIndex) {
61
        case 0:
62
            return x;
63
        case 1:
64
            return y;
65
        case 2:
66
            return z;
67
    }
68
    return x;
69
}
70

71
template<class float_type>
72
Vector3<float_type> Vector3<float_type>::operator+(const Vector3<float_type>& rcVct) const
73
{
74
    Vector3<float_type> cVctRes;
75
    cVctRes.x = x + rcVct.x;
76
    cVctRes.y = y + rcVct.y;
77
    cVctRes.z = z + rcVct.z;
78
    return cVctRes;
79
}
80

81
template<class float_type>
82
Vector3<float_type> Vector3<float_type>::operator&(const Vector3<float_type>& rcVct) const
83
{
84
    Vector3<float_type> cVctRes;
85
    cVctRes.x = x * (float_type)fabs(rcVct.x);
86
    cVctRes.y = y * (float_type)fabs(rcVct.y);
87
    cVctRes.z = z * (float_type)fabs(rcVct.z);
88
    return cVctRes;
89
}
90

91
template<class float_type>
92
Vector3<float_type> Vector3<float_type>::operator-(const Vector3<float_type>& rcVct) const
93
{
94
    Vector3<float_type> cVctRes;
95
    cVctRes.x = x - rcVct.x;
96
    cVctRes.y = y - rcVct.y;
97
    cVctRes.z = z - rcVct.z;
98
    return cVctRes;
99
}
100

101
template<class float_type>
102
Vector3<float_type> Vector3<float_type>::operator-() const
103
{
104
    return Vector3(-x, -y, -z);
105
}
106

107
template<class float_type>
108
Vector3<float_type>& Vector3<float_type>::operator+=(const Vector3<float_type>& rcVct)
109
{
110
    x += rcVct.x;
111
    y += rcVct.y;
112
    z += rcVct.z;
113
    return *this;
114
}
115

116
template<class float_type>
117
Vector3<float_type>& Vector3<float_type>::operator-=(const Vector3<float_type>& rcVct)
118
{
119
    x -= rcVct.x;
120
    y -= rcVct.y;
121
    z -= rcVct.z;
122
    return *this;
123
}
124

125
template<class float_type>
126
Vector3<float_type>& Vector3<float_type>::operator*=(float_type fScale)
127
{
128
    x *= fScale;
129
    y *= fScale;
130
    z *= fScale;
131
    return *this;
132
}
133

134
template<class float_type>
135
Vector3<float_type>& Vector3<float_type>::operator/=(float_type fDiv)
136
{
137
    x /= fDiv;
138
    y /= fDiv;
139
    z /= fDiv;
140
    return *this;
141
}
142

143
template<class float_type>
144
Vector3<float_type> Vector3<float_type>::operator*(float_type fScale) const
145
{
146
    return Vector3<float_type>(this->x * fScale, this->y * fScale, this->z * fScale);
147
}
148

149
template<class float_type>
150
Vector3<float_type> Vector3<float_type>::operator/(float_type fDiv) const
151
{
152
    return Vector3<float_type>(this->x / fDiv, this->y / fDiv, this->z / fDiv);
153
}
154

155
template<class float_type>
156
float_type Vector3<float_type>::operator*(const Vector3<float_type>& rcVct) const
157
{
158
    return (x * rcVct.x) + (y * rcVct.y) + (z * rcVct.z);
159
}
160

161
template<class float_type>
162
float_type Vector3<float_type>::Dot(const Vector3<float_type>& rcVct) const
163
{
164
    return (x * rcVct.x) + (y * rcVct.y) + (z * rcVct.z);
165
}
166

167
template<class float_type>
168
Vector3<float_type> Vector3<float_type>::operator%(const Vector3<float_type>& rcVct) const
169
{
170
    Vector3<float_type> cVctRes;
171
    cVctRes.x = (y * rcVct.z) - (z * rcVct.y);
172
    cVctRes.y = (z * rcVct.x) - (x * rcVct.z);
173
    cVctRes.z = (x * rcVct.y) - (y * rcVct.x);
174
    return cVctRes;
175
}
176

177
template<class float_type>
178
Vector3<float_type> Vector3<float_type>::Cross(const Vector3<float_type>& rcVct) const
179
{
180
    Vector3<float_type> cVctRes;
181
    cVctRes.x = (y * rcVct.z) - (z * rcVct.y);
182
    cVctRes.y = (z * rcVct.x) - (x * rcVct.z);
183
    cVctRes.z = (x * rcVct.y) - (y * rcVct.x);
184
    return cVctRes;
185
}
186

187
template<class float_type>
188
bool Vector3<float_type>::IsOnLineSegment(const Vector3<float_type>& startVct,
189
                                          const Vector3<float_type>& endVct) const
190
{
191
    Vector3<float_type> vectorAB = endVct - startVct;
192
    Vector3<float_type> vectorAC = *this - startVct;
193
    Vector3<float_type> crossproduct = vectorAB.Cross(vectorAC);
194
    float_type dotproduct = vectorAB.Dot(vectorAC);
195

196
    if (crossproduct.Length() > traits_type::epsilon()) {
197
        return false;
198
    }
199

200
    if (dotproduct < 0) {
201
        return false;
202
    }
203

204
    if (dotproduct > vectorAB.Sqr()) {
205
        return false;
206
    }
207

208
    return true;
209
}
210

211
template<class float_type>
212
bool Vector3<float_type>::operator!=(const Vector3<float_type>& rcVct) const
213
{
214
    return !((*this) == rcVct);
215
}
216

217
template<class float_type>
218
bool Vector3<float_type>::operator==(const Vector3<float_type>& rcVct) const
219
{
220
    return (std::fabs(x - rcVct.x) <= traits_type::epsilon())
221
        && (std::fabs(y - rcVct.y) <= traits_type::epsilon())
222
        && (std::fabs(z - rcVct.z) <= traits_type::epsilon());
223
}
224

225
template<class float_type>
226
bool Vector3<float_type>::IsEqual(const Vector3<float_type>& rclPnt, float_type tol) const
227
{
228
    return Distance(*this, rclPnt) <= tol;
229
}
230

231
template<class float_type>
232
bool Vector3<float_type>::IsParallel(const Vector3<float_type>& rclDir, float_type tol) const
233
{
234
    float_type angle = GetAngle(rclDir);
235
    if (boost::math::isnan(angle)) {
236
        return false;
237
    }
238

239
    return angle <= tol || traits_type::pi() - angle <= tol;
240
}
241

242
template<class float_type>
243
bool Vector3<float_type>::IsNormal(const Vector3<float_type>& rclDir, float_type tol) const
244
{
245
    float_type angle = GetAngle(rclDir);
246
    if (boost::math::isnan(angle)) {
247
        return false;
248
    }
249

250
    float_type diff = std::abs(traits_type::pi() / 2.0 - angle);  // NOLINT
251
    return diff <= tol;
252
}
253

254
template<class float_type>
255
Vector3<float_type>& Vector3<float_type>::ProjectToPlane(const Vector3<float_type>& rclBase,
256
                                                         const Vector3<float_type>& rclNorm)
257
{
258
    Vector3<float_type> clTemp(rclNorm);
259
    *this = *this - (clTemp *= ((*this - rclBase) * clTemp) / clTemp.Sqr());
260
    return *this;
261
}
262

263
template<class float_type>
264
void Vector3<float_type>::ProjectToPlane(const Vector3& rclBase,
265
                                         const Vector3& rclNorm,
266
                                         Vector3& rclProj) const
267
{
268
    Vector3<float_type> clTemp(rclNorm);
269
    rclProj = *this - (clTemp *= ((*this - rclBase) * clTemp) / clTemp.Sqr());
270
}
271

272
template<class float_type>
273
float_type Vector3<float_type>::DistanceToPlane(const Vector3<float_type>& rclBase,
274
                                                const Vector3<float_type>& rclNorm) const
275
{
276
    return ((*this - rclBase) * rclNorm) / rclNorm.Length();
277
}
278

279
template<class float_type>
280
float_type Vector3<float_type>::Length() const
281
{
282
    return static_cast<float_type>(std::sqrt((x * x) + (y * y) + (z * z)));
283
}
284

285
template<class float_type>
286
float_type Vector3<float_type>::DistanceToLine(const Vector3<float_type>& base,
287
                                               const Vector3<float_type>& dir) const
288
{
289
    // clang-format off
290
    return static_cast<float_type>(std::fabs((dir % Vector3(*this - base)).Length() / dir.Length()));
291
    // clang-format on
292
}
293

294
template<class float_type>
295
Vector3<float_type> Vector3<float_type>::DistanceToLineSegment(const Vector3& rclP1,
296
                                                               const Vector3& rclP2) const
297
{
298
    float_type len2 = Base::DistanceP2(rclP1, rclP2);
299
    if (len2 == 0) {
300
        return rclP1;
301
    }
302

303
    Vector3<float_type> p2p1 = rclP2 - rclP1;
304
    Vector3<float_type> pXp1 = *this - rclP1;
305
    float_type dot = pXp1 * p2p1;
306
    float_type t = clamp<float_type>(dot / len2, 0, 1);
307
    Vector3<float_type> dist = t * p2p1 - pXp1;
308
    return dist;
309
}
310

311
template<class float_type>
312
Vector3<float_type>& Vector3<float_type>::ProjectToLine(const Vector3<float_type>& rclPoint,
313
                                                        const Vector3<float_type>& rclLine)
314
{
315
    return (*this = ((((rclPoint * rclLine) / rclLine.Sqr()) * rclLine) - rclPoint));
316
}
317

318
template<class float_type>
319
Vector3<float_type> Vector3<float_type>::Perpendicular(const Vector3<float_type>& rclBase,
320
                                                       const Vector3<float_type>& rclDir) const
321
{
322
    float_type t = ((*this - rclBase) * rclDir) / (rclDir * rclDir);
323
    return rclBase + t * rclDir;
324
}
325

326
template<class float_type>
327
float_type Vector3<float_type>::Sqr() const
328
{
329
    return (float_type)((x * x) + (y * y) + (z * z));
330
}
331

332
template<class float_type>
333
void Vector3<float_type>::Set(float_type fX, float_type fY, float_type fZ)
334
{
335
    x = fX;
336
    y = fY;
337
    z = fZ;
338
}
339

340
template<class float_type>
341
void Vector3<float_type>::ScaleX(float_type f)
342
{
343
    x *= f;
344
}
345

346
template<class float_type>
347
void Vector3<float_type>::ScaleY(float_type f)
348
{
349
    y *= f;
350
}
351

352
template<class float_type>
353
void Vector3<float_type>::ScaleZ(float_type f)
354
{
355
    z *= f;
356
}
357

358
template<class float_type>
359
void Vector3<float_type>::Scale(float_type fX, float_type fY, float_type fZ)
360
{
361
    x *= fX;
362
    y *= fY;
363
    z *= fZ;
364
}
365

366
template<class float_type>
367
void Vector3<float_type>::MoveX(float_type f)
368
{
369
    x += f;
370
}
371

372
template<class float_type>
373
void Vector3<float_type>::MoveY(float_type f)
374
{
375
    y += f;
376
}
377

378
template<class float_type>
379
void Vector3<float_type>::MoveZ(float_type f)
380
{
381
    z += f;
382
}
383

384
template<class float_type>
385
void Vector3<float_type>::Move(float_type fX, float_type fY, float_type fZ)
386
{
387
    x += fX;
388
    y += fY;
389
    z += fZ;
390
}
391

392
template<class float_type>
393
void Vector3<float_type>::RotateX(float_type f)
394
{
395
    Vector3 cPt(*this);
396

397
    float_type fsin = static_cast<float_type>(sin(f));
398
    float_type fcos = static_cast<float_type>(cos(f));
399
    y = (cPt.y * fcos) - (cPt.z * fsin);
400
    z = (cPt.y * fsin) + (cPt.z * fcos);
401
}
402

403
template<class float_type>
404
void Vector3<float_type>::RotateY(float_type f)
405
{
406
    Vector3 cPt(*this);
407

408
    float_type fsin = static_cast<float_type>(sin(f));
409
    float_type fcos = static_cast<float_type>(cos(f));
410
    x = (cPt.z * fsin) + (cPt.x * fcos);
411
    z = (cPt.z * fcos) - (cPt.x * fsin);
412
}
413

414
template<class float_type>
415
void Vector3<float_type>::RotateZ(float_type f)
416
{
417
    Vector3 cPt(*this);
418

419
    float_type fsin = static_cast<float_type>(sin(f));
420
    float_type fcos = static_cast<float_type>(cos(f));
421
    x = (cPt.x * fcos) - (cPt.y * fsin);
422
    y = (cPt.x * fsin) + (cPt.y * fcos);
423
}
424

425
template<class float_type>
426
Vector3<float_type>& Vector3<float_type>::Normalize()
427
{
428
    float_type fLen = Length();
429
    if (fLen != static_cast<float_type>(0.0) && fLen != static_cast<float_type>(1.0)) {
430
        x /= fLen;
431
        y /= fLen;
432
        z /= fLen;
433
    }
434
    return *this;
435
}
436

437
template<class float_type>
438
bool Vector3<float_type>::IsNull() const
439
{
440
    float_type n {0.0};
441
    return (x == n) && (y == n) && (z == n);
442
}
443

444
template<class float_type>
445
float_type Vector3<float_type>::GetAngle(const Vector3& rcVect) const
446
{
447
    float_type len1 = Length();
448
    float_type len2 = rcVect.Length();
449
    if (len1 <= traits_type::epsilon() || len2 <= traits_type::epsilon()) {
450
        return std::numeric_limits<float_type>::quiet_NaN();  // division by zero
451
    }
452

453
    float_type dot = Dot(rcVect);
454
    dot /= len1;
455
    dot /= len2;
456

457
    if (dot <= -1.0) {
458
        return traits_type::pi();
459
    }
460
    if (dot >= 1.0) {
461
        return 0.0;
462
    }
463

464
    return float_type(acos(dot));
465
}
466

467
template<class float_type>
468
float_type Vector3<float_type>::GetAngleOriented(const Vector3& rcVect, const Vector3& norm) const
469
{
470
    float_type angle = GetAngle(rcVect);
471

472
    Vector3<float_type> crossProduct = Cross(rcVect);
473

474
    // Use dot product to determine the sign
475
    float_type dot = crossProduct.Dot(norm);
476
    if (dot < 0) {
477
        angle = 2 * traits_type::pi() - angle;
478
    }
479

480
    return angle;
481
}
482

483
template<class float_type>
484
void Vector3<float_type>::TransformToCoordinateSystem(const Vector3& rclBase,
485
                                                      const Vector3& rclDirX,
486
                                                      const Vector3& rclDirY)
487
{
488
    Vector3 clVectX;
489
    Vector3 clVectY;
490
    Vector3 clVectZ;
491
    Vector3 clVectOld;
492

493
    clVectX = rclDirX;
494
    clVectY = rclDirY;
495
    clVectZ = rclDirX % rclDirY;
496
    clVectX.Normalize();
497
    clVectY.Normalize();
498
    clVectZ.Normalize();
499

500
    clVectOld = *this - rclBase;
501

502
    x = clVectX * clVectOld;
503
    y = clVectY * clVectOld;
504
    z = clVectZ * clVectOld;
505
}
506

507
// explicit template instantiation
508
namespace Base
509
{
510
template class BaseExport Vector3<float>;
511
template class BaseExport Vector3<double>;
512
}  // namespace Base
513