FreeCAD

1
/***************************************************************************
2
 *   Copyright (c) 2014 Werner Mayer <wmayer[at]users.sourceforge.net>     *
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., 51 Franklin Street,      *
19
 *   Fifth Floor, Boston, MA  02110-1301, USA                              *
20
 *                                                                         *
21
 ***************************************************************************/
22

23

24
#include "PreCompiled.h"
25

26
#include "CoordinateSystem.h"
27
#include "Exception.h"
28
#include "Matrix.h"
29

30

31
using namespace Base;
32

33
CoordinateSystem::CoordinateSystem()
34
    : axis(Vector3d(), Vector3d(0, 0, 1))
35
    , xdir(1, 0, 0)
36
    , ydir(0, 1, 0)
37
{}
38

39
void CoordinateSystem::setAxes(const Axis& vec, const Vector3d& xd)
40
{
41
    if (xd.Sqr() < Base::Vector3d::epsilon()) {
42
        throw Base::ValueError("Direction is null vector");
43
    }
44
    Vector3d yd = vec.getDirection() % xd;
45
    if (yd.Sqr() < Base::Vector3d::epsilon()) {
46
        throw Base::ValueError("Direction is parallel to Z direction");
47
    }
48
    ydir = yd;
49
    ydir.Normalize();
50
    xdir = ydir % vec.getDirection();
51
    xdir.Normalize();
52
    axis.setBase(vec.getBase());
53
    Base::Vector3d zdir = vec.getDirection();
54
    zdir.Normalize();
55
    axis.setDirection(zdir);
56
}
57

58
void CoordinateSystem::setAxes(const Vector3d& n, const Vector3d& xd)
59
{
60
    if (xd.Sqr() < Base::Vector3d::epsilon()) {
61
        throw Base::ValueError("Direction is null vector");
62
    }
63
    Vector3d yd = n % xd;
64
    if (yd.Sqr() < Base::Vector3d::epsilon()) {
65
        throw Base::ValueError("Direction is parallel to Z direction");
66
    }
67
    ydir = yd;
68
    ydir.Normalize();
69
    xdir = ydir % n;
70
    xdir.Normalize();
71
    Base::Vector3d zdir = n;
72
    zdir.Normalize();
73
    axis.setDirection(zdir);
74
}
75

76
void CoordinateSystem::setAxis(const Axis& axis)
77
{
78
    setAxes(axis, xdir);
79
}
80

81
void CoordinateSystem::setXDirection(const Vector3d& dir)
82
{
83
    Vector3d yd = axis.getDirection() % dir;
84
    if (yd.Sqr() < Base::Vector3d::epsilon()) {
85
        throw Base::ValueError("Direction is parallel to Z direction");
86
    }
87
    ydir = yd;
88
    ydir.Normalize();
89
    xdir = ydir % axis.getDirection();
90
    xdir.Normalize();
91
}
92

93
void CoordinateSystem::setYDirection(const Vector3d& dir)
94
{
95
    Vector3d xd = dir % axis.getDirection();
96
    if (xd.Sqr() < Base::Vector3d::epsilon()) {
97
        throw Base::ValueError("Direction is parallel to Z direction");
98
    }
99
    xdir = xd;
100
    xdir.Normalize();
101
    ydir = axis.getDirection() % xdir;
102
    ydir.Normalize();
103
}
104

105
void CoordinateSystem::setZDirection(const Vector3d& dir)
106
{
107
    setAxes(dir, xdir);
108
}
109

110
Placement CoordinateSystem::displacement(const CoordinateSystem& cs) const
111
{
112
    // NOLINTBEGIN
113
    const Base::Vector3d& a = axis.getBase();
114
    const Base::Vector3d& zdir = axis.getDirection();
115
    Base::Matrix4D At;
116
    At[0][0] = xdir.x;
117
    At[1][0] = ydir.x;
118
    At[2][0] = zdir.x;
119
    At[0][1] = xdir.y;
120
    At[1][1] = ydir.y;
121
    At[2][1] = zdir.y;
122
    At[0][2] = xdir.z;
123
    At[1][2] = ydir.z;
124
    At[2][2] = zdir.z;
125
    Base::Vector3d at = At * a;
126
    At[0][3] = -at.x;
127
    At[1][3] = -at.y;
128
    At[2][3] = -at.z;
129

130
    const Base::Vector3d& b = cs.axis.getBase();
131
    const Base::Vector3d& cszdir = cs.axis.getDirection();
132
    Base::Matrix4D B;
133
    B[0][0] = cs.xdir.x;
134
    B[0][1] = cs.ydir.x;
135
    B[0][2] = cszdir.x;
136
    B[0][3] = b.x;
137
    B[1][0] = cs.xdir.y;
138
    B[1][1] = cs.ydir.y;
139
    B[1][2] = cszdir.y;
140
    B[1][3] = b.y;
141
    B[2][0] = cs.xdir.z;
142
    B[2][1] = cs.ydir.z;
143
    B[2][2] = cszdir.z;
144
    B[2][3] = b.z;
145

146
    Placement PAt(At);
147
    Placement PB(B);
148
    Placement C = PB * PAt;
149
    return C;
150
    // NOLINTEND
151
}
152

153
void CoordinateSystem::transformTo(Vector3d& pnt)
154
{
155
    return pnt.TransformToCoordinateSystem(axis.getBase(), xdir, ydir);
156
}
157

158
void CoordinateSystem::transform(const Placement& plm)
159
{
160
    axis *= plm;
161
    plm.getRotation().multVec(this->xdir, this->xdir);
162
    plm.getRotation().multVec(this->ydir, this->ydir);
163
}
164

165
void CoordinateSystem::transform(const Rotation& rot)
166
{
167
    Vector3d zdir = axis.getDirection();
168
    rot.multVec(zdir, zdir);
169
    axis.setDirection(zdir);
170
    rot.multVec(this->xdir, this->xdir);
171
    rot.multVec(this->ydir, this->ydir);
172
}
173

174
void CoordinateSystem::setPlacement(const Placement& plm)
175
{
176
    Vector3d zdir(0, 0, 1);
177
    plm.getRotation().multVec(zdir, zdir);
178
    axis.setBase(plm.getPosition());
179
    axis.setDirection(zdir);
180

181
    plm.getRotation().multVec(Vector3d(1, 0, 0), this->xdir);
182
    plm.getRotation().multVec(Vector3d(0, 1, 0), this->ydir);
183
}
184