1/**************************************************************************/
2/* transform_3d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "transform_3d.h"
32
33#include "core/math/math_funcs.h"
34#include "core/string/ustring.h"
35
36void Transform3D::affine_invert() {
37 basis.invert();
38 origin = basis.xform(-origin);
39}
40
41Transform3D Transform3D::affine_inverse() const {
42 Transform3D ret = *this;
43 ret.affine_invert();
44 return ret;
45}
46
47void Transform3D::invert() {
48 basis.transpose();
49 origin = basis.xform(-origin);
50}
51
52Transform3D Transform3D::inverse() const {
53 // FIXME: this function assumes the basis is a rotation matrix, with no scaling.
54 // Transform3D::affine_inverse can handle matrices with scaling, so GDScript should eventually use that.
55 Transform3D ret = *this;
56 ret.invert();
57 return ret;
58}
59
60void Transform3D::rotate(const Vector3 &p_axis, real_t p_angle) {
61 *this = rotated(p_axis, p_angle);
62}
63
64Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_angle) const {
65 // Equivalent to left multiplication
66 Basis p_basis(p_axis, p_angle);
67 return Transform3D(p_basis * basis, p_basis.xform(origin));
68}
69
70Transform3D Transform3D::rotated_local(const Vector3 &p_axis, real_t p_angle) const {
71 // Equivalent to right multiplication
72 Basis p_basis(p_axis, p_angle);
73 return Transform3D(basis * p_basis, origin);
74}
75
76void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_angle) {
77 basis.rotate(p_axis, p_angle);
78}
79
80Transform3D Transform3D::looking_at(const Vector3 &p_target, const Vector3 &p_up, bool p_use_model_front) const {
81#ifdef MATH_CHECKS
82 ERR_FAIL_COND_V_MSG(origin.is_equal_approx(p_target), Transform3D(), "The transform's origin and target can't be equal.");
83#endif
84 Transform3D t = *this;
85 t.basis = Basis::looking_at(p_target - origin, p_up, p_use_model_front);
86 return t;
87}
88
89void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up, bool p_use_model_front) {
90#ifdef MATH_CHECKS
91 ERR_FAIL_COND_MSG(p_eye.is_equal_approx(p_target), "The eye and target vectors can't be equal.");
92#endif
93 basis = Basis::looking_at(p_target - p_eye, p_up, p_use_model_front);
94 origin = p_eye;
95}
96
97Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const {
98 Transform3D interp;
99
100 Vector3 src_scale = basis.get_scale();
101 Quaternion src_rot = basis.get_rotation_quaternion();
102 Vector3 src_loc = origin;
103
104 Vector3 dst_scale = p_transform.basis.get_scale();
105 Quaternion dst_rot = p_transform.basis.get_rotation_quaternion();
106 Vector3 dst_loc = p_transform.origin;
107
108 interp.basis.set_quaternion_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c));
109 interp.origin = src_loc.lerp(dst_loc, p_c);
110
111 return interp;
112}
113
114void Transform3D::scale(const Vector3 &p_scale) {
115 basis.scale(p_scale);
116 origin *= p_scale;
117}
118
119Transform3D Transform3D::scaled(const Vector3 &p_scale) const {
120 // Equivalent to left multiplication
121 return Transform3D(basis.scaled(p_scale), origin * p_scale);
122}
123
124Transform3D Transform3D::scaled_local(const Vector3 &p_scale) const {
125 // Equivalent to right multiplication
126 return Transform3D(basis.scaled_local(p_scale), origin);
127}
128
129void Transform3D::scale_basis(const Vector3 &p_scale) {
130 basis.scale(p_scale);
131}
132
133void Transform3D::translate_local(real_t p_tx, real_t p_ty, real_t p_tz) {
134 translate_local(Vector3(p_tx, p_ty, p_tz));
135}
136
137void Transform3D::translate_local(const Vector3 &p_translation) {
138 for (int i = 0; i < 3; i++) {
139 origin[i] += basis[i].dot(p_translation);
140 }
141}
142
143Transform3D Transform3D::translated(const Vector3 &p_translation) const {
144 // Equivalent to left multiplication
145 return Transform3D(basis, origin + p_translation);
146}
147
148Transform3D Transform3D::translated_local(const Vector3 &p_translation) const {
149 // Equivalent to right multiplication
150 return Transform3D(basis, origin + basis.xform(p_translation));
151}
152
153void Transform3D::orthonormalize() {
154 basis.orthonormalize();
155}
156
157Transform3D Transform3D::orthonormalized() const {
158 Transform3D _copy = *this;
159 _copy.orthonormalize();
160 return _copy;
161}
162
163void Transform3D::orthogonalize() {
164 basis.orthogonalize();
165}
166
167Transform3D Transform3D::orthogonalized() const {
168 Transform3D _copy = *this;
169 _copy.orthogonalize();
170 return _copy;
171}
172
173bool Transform3D::is_equal_approx(const Transform3D &p_transform) const {
174 return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin);
175}
176
177bool Transform3D::is_finite() const {
178 return basis.is_finite() && origin.is_finite();
179}
180
181bool Transform3D::operator==(const Transform3D &p_transform) const {
182 return (basis == p_transform.basis && origin == p_transform.origin);
183}
184
185bool Transform3D::operator!=(const Transform3D &p_transform) const {
186 return (basis != p_transform.basis || origin != p_transform.origin);
187}
188
189void Transform3D::operator*=(const Transform3D &p_transform) {
190 origin = xform(p_transform.origin);
191 basis *= p_transform.basis;
192}
193
194Transform3D Transform3D::operator*(const Transform3D &p_transform) const {
195 Transform3D t = *this;
196 t *= p_transform;
197 return t;
198}
199
200void Transform3D::operator*=(const real_t p_val) {
201 origin *= p_val;
202 basis *= p_val;
203}
204
205Transform3D Transform3D::operator*(const real_t p_val) const {
206 Transform3D ret(*this);
207 ret *= p_val;
208 return ret;
209}
210
211Transform3D::operator String() const {
212 return "[X: " + basis.get_column(0).operator String() +
213 ", Y: " + basis.get_column(1).operator String() +
214 ", Z: " + basis.get_column(2).operator String() +
215 ", O: " + origin.operator String() + "]";
216}
217
218Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) :
219 basis(p_basis),
220 origin(p_origin) {
221}
222
223Transform3D::Transform3D(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z, const Vector3 &p_origin) :
224 origin(p_origin) {
225 basis.set_column(0, p_x);
226 basis.set_column(1, p_y);
227 basis.set_column(2, p_z);
228}
229
230Transform3D::Transform3D(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz, real_t ox, real_t oy, real_t oz) {
231 basis = Basis(xx, xy, xz, yx, yy, yz, zx, zy, zz);
232 origin = Vector3(ox, oy, oz);
233}
234