Relintai/rcpp_framework
1
0
Fork 0
mirror of https://github.com/Relintai/rcpp_framework.git synced 2025-04-20 01:43:12 +02:00
Code Issues Packages Projects Releases Wiki Activity

Added a few math classes from Godot.

Browse Source
This commit is contained in:
Relintai 2021-11-16 03:09:27 +01:00
parent ff6dd8ea5d
commit 19a70a5507
15 changed files with 4805 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

413
core/math/aabb.cpp Normal file
View File

@ -0,0 +1,413 @@
/*************************************************************************/
/* aabb.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "aabb.h"
#include "core/math/math.h"
real_t AABB::get_area() const {
return size.x * size.y * size.z;
}
bool AABB::operator==(const AABB &p_rval) const {
return ((position == p_rval.position) && (size == p_rval.size));
}
bool AABB::operator!=(const AABB &p_rval) const {
return ((position != p_rval.position) || (size != p_rval.size));
}
bool AABB::create_from_points(const Vector<Vector3> &p_points) {
if (!p_points.size()) {
return false;
}
Vector3 begin = p_points[0];
Vector3 end = begin;
for (int n = 1; n < p_points.size(); n++) {
const Vector3 &pt = p_points[n];
if (pt.x < begin.x) {
begin.x = pt.x;
}
if (pt.y < begin.y) {
begin.y = pt.y;
}
if (pt.z < begin.z) {
begin.z = pt.z;
}
if (pt.x > end.x) {
end.x = pt.x;
}
if (pt.y > end.y) {
end.y = pt.y;
}
if (pt.z > end.z) {
end.z = pt.z;
}
}
position = begin;
size = end - begin;
return true;
}
void AABB::merge_with(const AABB &p_aabb) {
Vector3 beg_1, beg_2;
Vector3 end_1, end_2;
Vector3 min, max;
beg_1 = position;
beg_2 = p_aabb.position;
end_1 = Vector3(size.x, size.y, size.z) + beg_1;
end_2 = Vector3(p_aabb.size.x, p_aabb.size.y, p_aabb.size.z) + beg_2;
min.x = (beg_1.x < beg_2.x) ? beg_1.x : beg_2.x;
min.y = (beg_1.y < beg_2.y) ? beg_1.y : beg_2.y;
min.z = (beg_1.z < beg_2.z) ? beg_1.z : beg_2.z;
max.x = (end_1.x > end_2.x) ? end_1.x : end_2.x;
max.y = (end_1.y > end_2.y) ? end_1.y : end_2.y;
max.z = (end_1.z > end_2.z) ? end_1.z : end_2.z;
position = min;
size = max - min;
}
bool AABB::is_equal_approx(const AABB &p_aabb) const {
return position.is_equal_approx(p_aabb.position) && size.is_equal_approx(p_aabb.size);
}
AABB AABB::intersection(const AABB &p_aabb) const {
Vector3 src_min = position;
Vector3 src_max = position + size;
Vector3 dst_min = p_aabb.position;
Vector3 dst_max = p_aabb.position + p_aabb.size;
Vector3 min, max;
if (src_min.x > dst_max.x || src_max.x < dst_min.x) {
return AABB();
} else {
min.x = (src_min.x > dst_min.x) ? src_min.x : dst_min.x;
max.x = (src_max.x < dst_max.x) ? src_max.x : dst_max.x;
}
if (src_min.y > dst_max.y || src_max.y < dst_min.y) {
return AABB();
} else {
min.y = (src_min.y > dst_min.y) ? src_min.y : dst_min.y;
max.y = (src_max.y < dst_max.y) ? src_max.y : dst_max.y;
}
if (src_min.z > dst_max.z || src_max.z < dst_min.z) {
return AABB();
} else {
min.z = (src_min.z > dst_min.z) ? src_min.z : dst_min.z;
max.z = (src_max.z < dst_max.z) ? src_max.z : dst_max.z;
}
return AABB(min, max - min);
}
bool AABB::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip, Vector3 *r_normal) const {
Vector3 c1, c2;
Vector3 end = position + size;
real_t near = -1e20;
real_t far = 1e20;
int axis = 0;
for (int i = 0; i < 3; i++) {
if (p_dir[i] == 0) {
if ((p_from[i] < position[i]) || (p_from[i] > end[i])) {
return false;
}
} else { // ray not parallel to planes in this direction
c1[i] = (position[i] - p_from[i]) / p_dir[i];
c2[i] = (end[i] - p_from[i]) / p_dir[i];
if (c1[i] > c2[i]) {
SWAP(c1, c2);
}
if (c1[i] > near) {
near = c1[i];
axis = i;
}
if (c2[i] < far) {
far = c2[i];
}
if ((near > far) || (far < 0)) {
return false;
}
}
}
if (r_clip) {
*r_clip = c1;
}
if (r_normal) {
*r_normal = Vector3();
(*r_normal)[axis] = p_dir[axis] ? -1 : 1;
}
return true;
}
bool AABB::intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip, Vector3 *r_normal) const {
real_t min = 0, max = 1;
int axis = 0;
real_t sign = 0;
for (int i = 0; i < 3; i++) {
real_t seg_from = p_from[i];
real_t seg_to = p_to[i];
real_t box_begin = position[i];
real_t box_end = box_begin + size[i];
real_t cmin, cmax;
real_t csign;
if (seg_from < seg_to) {
if (seg_from > box_end || seg_to < box_begin) {
return false;
}
real_t length = seg_to - seg_from;
cmin = (seg_from < box_begin) ? ((box_begin - seg_from) / length) : 0;
cmax = (seg_to > box_end) ? ((box_end - seg_from) / length) : 1;
csign = -1.0;
} else {
if (seg_to > box_end || seg_from < box_begin) {
return false;
}
real_t length = seg_to - seg_from;
cmin = (seg_from > box_end) ? (box_end - seg_from) / length : 0;
cmax = (seg_to < box_begin) ? (box_begin - seg_from) / length : 1;
csign = 1.0;
}
if (cmin > min) {
min = cmin;
axis = i;
sign = csign;
}
if (cmax < max) {
max = cmax;
}
if (max < min) {
return false;
}
}
Vector3 rel = p_to - p_from;
if (r_normal) {
Vector3 normal;
normal[axis] = sign;
*r_normal = normal;
}
if (r_clip) {
*r_clip = p_from + rel * min;
}
return true;
}
bool AABB::intersects_plane(const Plane &p_plane) const {
Vector3 points[8] = {
Vector3(position.x, position.y, position.z),
Vector3(position.x, position.y, position.z + size.z),
Vector3(position.x, position.y + size.y, position.z),
Vector3(position.x, position.y + size.y, position.z + size.z),
Vector3(position.x + size.x, position.y, position.z),
Vector3(position.x + size.x, position.y, position.z + size.z),
Vector3(position.x + size.x, position.y + size.y, position.z),
Vector3(position.x + size.x, position.y + size.y, position.z + size.z),
};
bool over = false;
bool under = false;
for (int i = 0; i < 8; i++) {
if (p_plane.distance_to(points[i]) > 0) {
over = true;
} else {
under = true;
}
}
return under && over;
}
Vector3 AABB::get_longest_axis() const {
Vector3 axis(1, 0, 0);
real_t max_size = size.x;
if (size.y > max_size) {
axis = Vector3(0, 1, 0);
max_size = size.y;
}
if (size.z > max_size) {
axis = Vector3(0, 0, 1);
}
return axis;
}
int AABB::get_longest_axis_index() const {
int axis = 0;
real_t max_size = size.x;
if (size.y > max_size) {
axis = 1;
max_size = size.y;
}
if (size.z > max_size) {
axis = 2;
}
return axis;
}
Vector3 AABB::get_shortest_axis() const {
Vector3 axis(1, 0, 0);
real_t max_size = size.x;
if (size.y < max_size) {
axis = Vector3(0, 1, 0);
max_size = size.y;
}
if (size.z < max_size) {
axis = Vector3(0, 0, 1);
}
return axis;
}
int AABB::get_shortest_axis_index() const {
int axis = 0;
real_t max_size = size.x;
if (size.y < max_size) {
axis = 1;
max_size = size.y;
}
if (size.z < max_size) {
axis = 2;
}
return axis;
}
AABB AABB::merge(const AABB &p_with) const {
AABB aabb = *this;
aabb.merge_with(p_with);
return aabb;
}
AABB AABB::expand(const Vector3 &p_vector) const {
AABB aabb = *this;
aabb.expand_to(p_vector);
return aabb;
}
AABB AABB::grow(real_t p_by) const {
AABB aabb = *this;
aabb.grow_by(p_by);
return aabb;
}
void AABB::get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const {
ERR_FAIL_INDEX(p_edge, 12);
switch (p_edge) {
case 0: {
r_from = Vector3(position.x + size.x, position.y, position.z);
r_to = Vector3(position.x, position.y, position.z);
} break;
case 1: {
r_from = Vector3(position.x + size.x, position.y, position.z + size.z);
r_to = Vector3(position.x + size.x, position.y, position.z);
} break;
case 2: {
r_from = Vector3(position.x, position.y, position.z + size.z);
r_to = Vector3(position.x + size.x, position.y, position.z + size.z);
} break;
case 3: {
r_from = Vector3(position.x, position.y, position.z);
r_to = Vector3(position.x, position.y, position.z + size.z);
} break;
case 4: {
r_from = Vector3(position.x, position.y + size.y, position.z);
r_to = Vector3(position.x + size.x, position.y + size.y, position.z);
} break;
case 5: {
r_from = Vector3(position.x + size.x, position.y + size.y, position.z);
r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
} break;
case 6: {
r_from = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
r_to = Vector3(position.x, position.y + size.y, position.z + size.z);
} break;
case 7: {
r_from = Vector3(position.x, position.y + size.y, position.z + size.z);
r_to = Vector3(position.x, position.y + size.y, position.z);
} break;
case 8: {
r_from = Vector3(position.x, position.y, position.z + size.z);
r_to = Vector3(position.x, position.y + size.y, position.z + size.z);
} break;
case 9: {
r_from = Vector3(position.x, position.y, position.z);
r_to = Vector3(position.x, position.y + size.y, position.z);
} break;
case 10: {
r_from = Vector3(position.x + size.x, position.y, position.z);
r_to = Vector3(position.x + size.x, position.y + size.y, position.z);
} break;
case 11: {
r_from = Vector3(position.x + size.x, position.y, position.z + size.z);
r_to = Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
} break;
}
}
AABB::operator String() const {
return String() + position + " - " + size;
}

425
core/math/aabb.h Normal file
View File

@ -0,0 +1,425 @@
/*************************************************************************/
/* aabb.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef AABB_H
#define AABB_H
#include "core/math/math_defs.h"
#include "core/math/plane.h"
#include "core/math/vector3.h"
#include "core/typedefs.h"
#include "core/containers/vector.h"
#include "core/string.h"
/**
* AABB / AABB (Axis Aligned Bounding Box)
* This is implemented by a point (position) and the box size
*/
class AABB {
public:
Vector3 position;
Vector3 size;
real_t get_area() const; /// get area
_FORCE_INLINE_ bool has_no_area() const {
return (size.x <= 0 || size.y <= 0 || size.z <= 0);
}
_FORCE_INLINE_ bool has_no_surface() const {
return (size.x <= 0 && size.y <= 0 && size.z <= 0);
}
const Vector3 &get_position() const { return position; }
void set_position(const Vector3 &p_pos) { position = p_pos; }
const Vector3 &get_size() const { return size; }
void set_size(const Vector3 &p_size) { size = p_size; }
Vector3 get_center() const { return position + (size * 0.5); }
bool operator==(const AABB &p_rval) const;
bool operator!=(const AABB &p_rval) const;
bool is_equal_approx(const AABB &p_aabb) const;
_FORCE_INLINE_ bool intersects(const AABB &p_aabb) const; /// Both AABBs overlap
_FORCE_INLINE_ bool intersects_inclusive(const AABB &p_aabb) const; /// Both AABBs (or their faces) overlap
_FORCE_INLINE_ bool encloses(const AABB &p_aabb) const; /// p_aabb is completely inside this
AABB merge(const AABB &p_with) const;
void merge_with(const AABB &p_aabb); ///merge with another AABB
AABB intersection(const AABB &p_aabb) const; ///get box where two intersect, empty if no intersection occurs
bool intersects_segment(const Vector3 &p_from, const Vector3 &p_to, Vector3 *r_clip = nullptr, Vector3 *r_normal = nullptr) const;
bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *r_clip = nullptr, Vector3 *r_normal = nullptr) const;
_FORCE_INLINE_ bool smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t t0, real_t t1) const;
_FORCE_INLINE_ bool intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const;
_FORCE_INLINE_ bool inside_convex_shape(const Plane *p_planes, int p_plane_count) const;
bool intersects_plane(const Plane &p_plane) const;
_FORCE_INLINE_ bool has_point(const Vector3 &p_point) const;
_FORCE_INLINE_ Vector3 get_support(const Vector3 &p_normal) const;
Vector3 get_longest_axis() const;
int get_longest_axis_index() const;
_FORCE_INLINE_ real_t get_longest_axis_size() const;
Vector3 get_shortest_axis() const;
int get_shortest_axis_index() const;
_FORCE_INLINE_ real_t get_shortest_axis_size() const;
AABB grow(real_t p_by) const;
_FORCE_INLINE_ void grow_by(real_t p_amount);
void get_edge(int p_edge, Vector3 &r_from, Vector3 &r_to) const;
_FORCE_INLINE_ Vector3 get_endpoint(int p_point) const;
AABB expand(const Vector3 &p_vector) const;
_FORCE_INLINE_ void project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const;
_FORCE_INLINE_ void expand_to(const Vector3 &p_vector); /** expand to contain a point if necessary */
bool create_from_points(const Vector<Vector3> &p_points);
_FORCE_INLINE_ AABB abs() const {
return AABB(Vector3(position.x + MIN(size.x, 0), position.y + MIN(size.y, 0), position.z + MIN(size.z, 0)), size.abs());
}
operator String() const;
_FORCE_INLINE_ AABB() {}
inline AABB(const Vector3 &p_pos, const Vector3 &p_size) :
position(p_pos),
size(p_size) {
}
};
inline bool AABB::intersects(const AABB &p_aabb) const {
if (position.x >= (p_aabb.position.x + p_aabb.size.x)) {
return false;
}
if ((position.x + size.x) <= p_aabb.position.x) {
return false;
}
if (position.y >= (p_aabb.position.y + p_aabb.size.y)) {
return false;
}
if ((position.y + size.y) <= p_aabb.position.y) {
return false;
}
if (position.z >= (p_aabb.position.z + p_aabb.size.z)) {
return false;
}
if ((position.z + size.z) <= p_aabb.position.z) {
return false;
}
return true;
}
inline bool AABB::intersects_inclusive(const AABB &p_aabb) const {
if (position.x > (p_aabb.position.x + p_aabb.size.x)) {
return false;
}
if ((position.x + size.x) < p_aabb.position.x) {
return false;
}
if (position.y > (p_aabb.position.y + p_aabb.size.y)) {
return false;
}
if ((position.y + size.y) < p_aabb.position.y) {
return false;
}
if (position.z > (p_aabb.position.z + p_aabb.size.z)) {
return false;
}
if ((position.z + size.z) < p_aabb.position.z) {
return false;
}
return true;
}
inline bool AABB::encloses(const AABB &p_aabb) const {
Vector3 src_min = position;
Vector3 src_max = position + size;
Vector3 dst_min = p_aabb.position;
Vector3 dst_max = p_aabb.position + p_aabb.size;
return (
(src_min.x <= dst_min.x) &&
(src_max.x > dst_max.x) &&
(src_min.y <= dst_min.y) &&
(src_max.y > dst_max.y) &&
(src_min.z <= dst_min.z) &&
(src_max.z > dst_max.z));
}
Vector3 AABB::get_support(const Vector3 &p_normal) const {
Vector3 half_extents = size * 0.5;
Vector3 ofs = position + half_extents;
return Vector3(
(p_normal.x > 0) ? -half_extents.x : half_extents.x,
(p_normal.y > 0) ? -half_extents.y : half_extents.y,
(p_normal.z > 0) ? -half_extents.z : half_extents.z) +
ofs;
}
Vector3 AABB::get_endpoint(int p_point) const {
switch (p_point) {
case 0:
return Vector3(position.x, position.y, position.z);
case 1:
return Vector3(position.x, position.y, position.z + size.z);
case 2:
return Vector3(position.x, position.y + size.y, position.z);
case 3:
return Vector3(position.x, position.y + size.y, position.z + size.z);
case 4:
return Vector3(position.x + size.x, position.y, position.z);
case 5:
return Vector3(position.x + size.x, position.y, position.z + size.z);
case 6:
return Vector3(position.x + size.x, position.y + size.y, position.z);
case 7:
return Vector3(position.x + size.x, position.y + size.y, position.z + size.z);
};
//ERR_FAIL_V(Vector3());
return Vector3();
}
bool AABB::intersects_convex_shape(const Plane *p_planes, int p_plane_count, const Vector3 *p_points, int p_point_count) const {
Vector3 half_extents = size * 0.5;
Vector3 ofs = position + half_extents;
for (int i = 0; i < p_plane_count; i++) {
const Plane &p = p_planes[i];
Vector3 point(
(p.normal.x > 0) ? -half_extents.x : half_extents.x,
(p.normal.y > 0) ? -half_extents.y : half_extents.y,
(p.normal.z > 0) ? -half_extents.z : half_extents.z);
point += ofs;
if (p.is_point_over(point)) {
return false;
}
}
// Make sure all points in the shape aren't fully separated from the AABB on
// each axis.
int bad_point_counts_positive[3] = { 0 };
int bad_point_counts_negative[3] = { 0 };
for (int k = 0; k < 3; k++) {
for (int i = 0; i < p_point_count; i++) {
if (p_points[i].coordinates[k] > ofs.coordinates[k] + half_extents.coordinates[k]) {
bad_point_counts_positive[k]++;
}
if (p_points[i].coordinates[k] < ofs.coordinates[k] - half_extents.coordinates[k]) {
bad_point_counts_negative[k]++;
}
}
if (bad_point_counts_negative[k] == p_point_count) {
return false;
}
if (bad_point_counts_positive[k] == p_point_count) {
return false;
}
}
return true;
}
bool AABB::inside_convex_shape(const Plane *p_planes, int p_plane_count) const {
Vector3 half_extents = size * 0.5;
Vector3 ofs = position + half_extents;
for (int i = 0; i < p_plane_count; i++) {
const Plane &p = p_planes[i];
Vector3 point(
(p.normal.x < 0) ? -half_extents.x : half_extents.x,
(p.normal.y < 0) ? -half_extents.y : half_extents.y,
(p.normal.z < 0) ? -half_extents.z : half_extents.z);
point += ofs;
if (p.is_point_over(point)) {
return false;
}
}
return true;
}
bool AABB::has_point(const Vector3 &p_point) const {
if (p_point.x < position.x) {
return false;
}
if (p_point.y < position.y) {
return false;
}
if (p_point.z < position.z) {
return false;
}
if (p_point.x > position.x + size.x) {
return false;
}
if (p_point.y > position.y + size.y) {
return false;
}
if (p_point.z > position.z + size.z) {
return false;
}
return true;
}
inline void AABB::expand_to(const Vector3 &p_vector) {
Vector3 begin = position;
Vector3 end = position + size;
if (p_vector.x < begin.x) {
begin.x = p_vector.x;
}
if (p_vector.y < begin.y) {
begin.y = p_vector.y;
}
if (p_vector.z < begin.z) {
begin.z = p_vector.z;
}
if (p_vector.x > end.x) {
end.x = p_vector.x;
}
if (p_vector.y > end.y) {
end.y = p_vector.y;
}
if (p_vector.z > end.z) {
end.z = p_vector.z;
}
position = begin;
size = end - begin;
}
void AABB::project_range_in_plane(const Plane &p_plane, real_t &r_min, real_t &r_max) const {
Vector3 half_extents(size.x * 0.5, size.y * 0.5, size.z * 0.5);
Vector3 center(position.x + half_extents.x, position.y + half_extents.y, position.z + half_extents.z);
real_t length = p_plane.normal.abs().dot(half_extents);
real_t distance = p_plane.distance_to(center);
r_min = distance - length;
r_max = distance + length;
}
inline real_t AABB::get_longest_axis_size() const {
real_t max_size = size.x;
if (size.y > max_size) {
max_size = size.y;
}
if (size.z > max_size) {
max_size = size.z;
}
return max_size;
}
inline real_t AABB::get_shortest_axis_size() const {
real_t max_size = size.x;
if (size.y < max_size) {
max_size = size.y;
}
if (size.z < max_size) {
max_size = size.z;
}
return max_size;
}
bool AABB::smits_intersect_ray(const Vector3 &p_from, const Vector3 &p_dir, real_t t0, real_t t1) const {
real_t divx = 1.0 / p_dir.x;
real_t divy = 1.0 / p_dir.y;
real_t divz = 1.0 / p_dir.z;
Vector3 upbound = position + size;
real_t tmin, tmax, tymin, tymax, tzmin, tzmax;
if (p_dir.x >= 0) {
tmin = (position.x - p_from.x) * divx;
tmax = (upbound.x - p_from.x) * divx;
} else {
tmin = (upbound.x - p_from.x) * divx;
tmax = (position.x - p_from.x) * divx;
}
if (p_dir.y >= 0) {
tymin = (position.y - p_from.y) * divy;
tymax = (upbound.y - p_from.y) * divy;
} else {
tymin = (upbound.y - p_from.y) * divy;
tymax = (position.y - p_from.y) * divy;
}
if ((tmin > tymax) || (tymin > tmax)) {
return false;
}
if (tymin > tmin) {
tmin = tymin;
}
if (tymax < tmax) {
tmax = tymax;
}
if (p_dir.z >= 0) {
tzmin = (position.z - p_from.z) * divz;
tzmax = (upbound.z - p_from.z) * divz;
} else {
tzmin = (upbound.z - p_from.z) * divz;
tzmax = (position.z - p_from.z) * divz;
}
if ((tmin > tzmax) || (tzmin > tmax)) {
return false;
}
if (tzmin > tmin) {
tmin = tzmin;
}
if (tzmax < tmax) {
tmax = tzmax;
}
return ((tmin < t1) && (tmax > t0));
}
void AABB::grow_by(real_t p_amount) {
position.x -= p_amount;
position.y -= p_amount;
position.z -= p_amount;
size.x += 2.0 * p_amount;
size.y += 2.0 * p_amount;
size.z += 2.0 * p_amount;
}
#endif // AABB_H

1023
core/math/basis.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

329
core/math/basis.h Normal file
View File

@ -0,0 +1,329 @@
/*************************************************************************/
/* basis.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef BASIS_H
#define BASIS_H
#include "core/math/quat.h"
#include "core/math/vector3.h"
class Basis {
public:
Vector3 elements[3] = {
Vector3(1, 0, 0),
Vector3(0, 1, 0),
Vector3(0, 0, 1)
};
_FORCE_INLINE_ const Vector3 &operator[](int axis) const {
return elements[axis];
}
_FORCE_INLINE_ Vector3 &operator[](int axis) {
return elements[axis];
}
void invert();
void transpose();
Basis inverse() const;
Basis transposed() const;
_FORCE_INLINE_ real_t determinant() const;
void from_z(const Vector3 &p_z);
_FORCE_INLINE_ Vector3 get_axis(int p_axis) const {
// get actual basis axis (elements is transposed for performance)
return Vector3(elements[0][p_axis], elements[1][p_axis], elements[2][p_axis]);
}
_FORCE_INLINE_ void set_axis(int p_axis, const Vector3 &p_value) {
// get actual basis axis (elements is transposed for performance)
elements[0][p_axis] = p_value.x;
elements[1][p_axis] = p_value.y;
elements[2][p_axis] = p_value.z;
}
void rotate(const Vector3 &p_axis, real_t p_phi);
Basis rotated(const Vector3 &p_axis, real_t p_phi) const;
void rotate_local(const Vector3 &p_axis, real_t p_phi);
Basis rotated_local(const Vector3 &p_axis, real_t p_phi) const;
void rotate(const Vector3 &p_euler);
Basis rotated(const Vector3 &p_euler) const;
void rotate(const Quat &p_quat);
Basis rotated(const Quat &p_quat) const;
Vector3 get_rotation_euler() const;
void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const;
void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const;
Quat get_rotation_quat() const;
Vector3 get_rotation() const { return get_rotation_euler(); };
Vector3 rotref_posscale_decomposition(Basis &rotref) const;
Vector3 get_euler_xyz() const;
void set_euler_xyz(const Vector3 &p_euler);
Vector3 get_euler_xzy() const;
void set_euler_xzy(const Vector3 &p_euler);
Vector3 get_euler_yzx() const;
void set_euler_yzx(const Vector3 &p_euler);
Vector3 get_euler_yxz() const;
void set_euler_yxz(const Vector3 &p_euler);
Vector3 get_euler_zxy() const;
void set_euler_zxy(const Vector3 &p_euler);
Vector3 get_euler_zyx() const;
void set_euler_zyx(const Vector3 &p_euler);
Quat get_quat() const;
void set_quat(const Quat &p_quat);
Vector3 get_euler() const { return get_euler_yxz(); }
void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }
void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const;
void set_axis_angle(const Vector3 &p_axis, real_t p_phi);
void scale(const Vector3 &p_scale);
Basis scaled(const Vector3 &p_scale) const;
void scale_local(const Vector3 &p_scale);
Basis scaled_local(const Vector3 &p_scale) const;
Vector3 get_scale() const;
Vector3 get_scale_abs() const;
Vector3 get_scale_local() const;
void set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale);
void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale);
void set_quat_scale(const Quat &p_quat, const Vector3 &p_scale);
// transposed dot products
_FORCE_INLINE_ real_t tdotx(const Vector3 &v) const {
return elements[0][0] * v[0] + elements[1][0] * v[1] + elements[2][0] * v[2];
}
_FORCE_INLINE_ real_t tdoty(const Vector3 &v) const {
return elements[0][1] * v[0] + elements[1][1] * v[1] + elements[2][1] * v[2];
}
_FORCE_INLINE_ real_t tdotz(const Vector3 &v) const {
return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2];
}
bool is_equal_approx(const Basis &p_basis) const;
// For complicated reasons, the second argument is always discarded. See #45062.
bool is_equal_approx(const Basis &a, const Basis &b) const { return is_equal_approx(a); }
bool is_equal_approx_ratio(const Basis &a, const Basis &b, real_t p_epsilon = UNIT_EPSILON) const;
bool operator==(const Basis &p_matrix) const;
bool operator!=(const Basis &p_matrix) const;
_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
_FORCE_INLINE_ void operator*=(const Basis &p_matrix);
_FORCE_INLINE_ Basis operator*(const Basis &p_matrix) const;
_FORCE_INLINE_ void operator+=(const Basis &p_matrix);
_FORCE_INLINE_ Basis operator+(const Basis &p_matrix) const;
_FORCE_INLINE_ void operator-=(const Basis &p_matrix);
_FORCE_INLINE_ Basis operator-(const Basis &p_matrix) const;
_FORCE_INLINE_ void operator*=(real_t p_val);
_FORCE_INLINE_ Basis operator*(real_t p_val) const;
int get_orthogonal_index() const;
void set_orthogonal_index(int p_index);
void set_diagonal(const Vector3 &p_diag);
bool is_orthogonal() const;
bool is_diagonal() const;
bool is_rotation() const;
Basis slerp(const Basis &p_to, const real_t &p_weight) const;
operator String() const;
/* create / set */
_FORCE_INLINE_ void set(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) {
elements[0][0] = xx;
elements[0][1] = xy;
elements[0][2] = xz;
elements[1][0] = yx;
elements[1][1] = yy;
elements[1][2] = yz;
elements[2][0] = zx;
elements[2][1] = zy;
elements[2][2] = zz;
}
_FORCE_INLINE_ void set(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) {
set_axis(0, p_x);
set_axis(1, p_y);
set_axis(2, p_z);
}
_FORCE_INLINE_ Vector3 get_column(int i) const {
return Vector3(elements[0][i], elements[1][i], elements[2][i]);
}
_FORCE_INLINE_ Vector3 get_row(int i) const {
return Vector3(elements[i][0], elements[i][1], elements[i][2]);
}
_FORCE_INLINE_ Vector3 get_main_diagonal() const {
return Vector3(elements[0][0], elements[1][1], elements[2][2]);
}
_FORCE_INLINE_ void set_row(int i, const Vector3 &p_row) {
elements[i][0] = p_row.x;
elements[i][1] = p_row.y;
elements[i][2] = p_row.z;
}
_FORCE_INLINE_ void set_zero() {
elements[0].zero();
elements[1].zero();
elements[2].zero();
}
_FORCE_INLINE_ Basis transpose_xform(const Basis &m) const {
return Basis(
elements[0].x * m[0].x + elements[1].x * m[1].x + elements[2].x * m[2].x,
elements[0].x * m[0].y + elements[1].x * m[1].y + elements[2].x * m[2].y,
elements[0].x * m[0].z + elements[1].x * m[1].z + elements[2].x * m[2].z,
elements[0].y * m[0].x + elements[1].y * m[1].x + elements[2].y * m[2].x,
elements[0].y * m[0].y + elements[1].y * m[1].y + elements[2].y * m[2].y,
elements[0].y * m[0].z + elements[1].y * m[1].z + elements[2].y * m[2].z,
elements[0].z * m[0].x + elements[1].z * m[1].x + elements[2].z * m[2].x,
elements[0].z * m[0].y + elements[1].z * m[1].y + elements[2].z * m[2].y,
elements[0].z * m[0].z + elements[1].z * m[1].z + elements[2].z * m[2].z);
}
Basis(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) {
set(xx, xy, xz, yx, yy, yz, zx, zy, zz);
}
void orthonormalize();
Basis orthonormalized() const;
bool is_symmetric() const;
Basis diagonalize();
operator Quat() const { return get_quat(); }
Basis(const Quat &p_quat) { set_quat(p_quat); };
Basis(const Quat &p_quat, const Vector3 &p_scale) { set_quat_scale(p_quat, p_scale); }
Basis(const Vector3 &p_euler) { set_euler(p_euler); }
Basis(const Vector3 &p_euler, const Vector3 &p_scale) { set_euler_scale(p_euler, p_scale); }
Basis(const Vector3 &p_axis, real_t p_phi) { set_axis_angle(p_axis, p_phi); }
Basis(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_phi, p_scale); }
_FORCE_INLINE_ Basis(const Vector3 &row0, const Vector3 &row1, const Vector3 &row2) {
elements[0] = row0;
elements[1] = row1;
elements[2] = row2;
}
_FORCE_INLINE_ Basis() {}
};
_FORCE_INLINE_ void Basis::operator*=(const Basis &p_matrix) {
set(
p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]),
p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]),
p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2]));
}
_FORCE_INLINE_ Basis Basis::operator*(const Basis &p_matrix) const {
return Basis(
p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]),
p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]),
p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2]));
}
_FORCE_INLINE_ void Basis::operator+=(const Basis &p_matrix) {
elements[0] += p_matrix.elements[0];
elements[1] += p_matrix.elements[1];
elements[2] += p_matrix.elements[2];
}
_FORCE_INLINE_ Basis Basis::operator+(const Basis &p_matrix) const {
Basis ret(*this);
ret += p_matrix;
return ret;
}
_FORCE_INLINE_ void Basis::operator-=(const Basis &p_matrix) {
elements[0] -= p_matrix.elements[0];
elements[1] -= p_matrix.elements[1];
elements[2] -= p_matrix.elements[2];
}
_FORCE_INLINE_ Basis Basis::operator-(const Basis &p_matrix) const {
Basis ret(*this);
ret -= p_matrix;
return ret;
}
_FORCE_INLINE_ void Basis::operator*=(real_t p_val) {
elements[0] *= p_val;
elements[1] *= p_val;
elements[2] *= p_val;
}
_FORCE_INLINE_ Basis Basis::operator*(real_t p_val) const {
Basis ret(*this);
ret *= p_val;
return ret;
}
Vector3 Basis::xform(const Vector3 &p_vector) const {
return Vector3(
elements[0].dot(p_vector),
elements[1].dot(p_vector),
elements[2].dot(p_vector));
}
Vector3 Basis::xform_inv(const Vector3 &p_vector) const {
return Vector3(
(elements[0][0] * p_vector.x) + (elements[1][0] * p_vector.y) + (elements[2][0] * p_vector.z),
(elements[0][1] * p_vector.x) + (elements[1][1] * p_vector.y) + (elements[2][1] * p_vector.z),
(elements[0][2] * p_vector.x) + (elements[1][2] * p_vector.y) + (elements[2][2] * p_vector.z));
}
real_t Basis::determinant() const {
return elements[0][0] * (elements[1][1] * elements[2][2] - elements[2][1] * elements[1][2]) -
elements[1][0] * (elements[0][1] * elements[2][2] - elements[2][1] * elements[0][2]) +
elements[2][0] * (elements[0][1] * elements[1][2] - elements[1][1] * elements[0][2]);
}
#endif // BASIS_H

664
core/math/camera_matrix.cpp Normal file
View File

@ -0,0 +1,664 @@
/*************************************************************************/
/* camera_matrix.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "camera_matrix.h"
#include "core/math/math.h"
void CameraMatrix::set_identity() {
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
matrix[i][j] = (i == j) ? 1 : 0;
}
}
}
void CameraMatrix::set_zero() {
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
matrix[i][j] = 0;
}
}
}
Plane CameraMatrix::xform4(const Plane &p_vec4) const {
Plane ret;
ret.normal.x = matrix[0][0] * p_vec4.normal.x + matrix[1][0] * p_vec4.normal.y + matrix[2][0] * p_vec4.normal.z + matrix[3][0] * p_vec4.d;
ret.normal.y = matrix[0][1] * p_vec4.normal.x + matrix[1][1] * p_vec4.normal.y + matrix[2][1] * p_vec4.normal.z + matrix[3][1] * p_vec4.d;
ret.normal.z = matrix[0][2] * p_vec4.normal.x + matrix[1][2] * p_vec4.normal.y + matrix[2][2] * p_vec4.normal.z + matrix[3][2] * p_vec4.d;
ret.d = matrix[0][3] * p_vec4.normal.x + matrix[1][3] * p_vec4.normal.y + matrix[2][3] * p_vec4.normal.z + matrix[3][3] * p_vec4.d;
return ret;
}
void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov) {
if (p_flip_fov) {
p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect);
}
real_t sine, cotangent, deltaZ;
real_t radians = p_fovy_degrees / 2.0 * Math_PI / 180.0;
deltaZ = p_z_far - p_z_near;
sine = Math::sin(radians);
if ((deltaZ == 0) || (sine == 0) || (p_aspect == 0)) {
return;
}
cotangent = Math::cos(radians) / sine;
set_identity();
matrix[0][0] = cotangent / p_aspect;
matrix[1][1] = cotangent;
matrix[2][2] = -(p_z_far + p_z_near) / deltaZ;
matrix[2][3] = -1;
matrix[3][2] = -2 * p_z_near * p_z_far / deltaZ;
matrix[3][3] = 0;
}
void CameraMatrix::set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist) {
if (p_flip_fov) {
p_fovy_degrees = get_fovy(p_fovy_degrees, 1.0 / p_aspect);
}
real_t left, right, modeltranslation, ymax, xmax, frustumshift;
ymax = p_z_near * tan(p_fovy_degrees * Math_PI / 360.0f);
xmax = ymax * p_aspect;
frustumshift = (p_intraocular_dist / 2.0) * p_z_near / p_convergence_dist;
switch (p_eye) {
case 1: { // left eye
left = -xmax + frustumshift;
right = xmax + frustumshift;
modeltranslation = p_intraocular_dist / 2.0;
}; break;
case 2: { // right eye
left = -xmax - frustumshift;
right = xmax - frustumshift;
modeltranslation = -p_intraocular_dist / 2.0;
}; break;
default: { // mono, should give the same result as set_perspective(p_fovy_degrees,p_aspect,p_z_near,p_z_far,p_flip_fov)
left = -xmax;
right = xmax;
modeltranslation = 0.0;
}; break;
};
set_frustum(left, right, -ymax, ymax, p_z_near, p_z_far);
// translate matrix by (modeltranslation, 0.0, 0.0)
CameraMatrix cm;
cm.set_identity();
cm.matrix[3][0] = modeltranslation;
*this = *this * cm;
}
void CameraMatrix::set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far) {
// we first calculate our base frustum on our values without taking our lens magnification into account.
real_t f1 = (p_intraocular_dist * 0.5) / p_display_to_lens;
real_t f2 = ((p_display_width - p_intraocular_dist) * 0.5) / p_display_to_lens;
real_t f3 = (p_display_width / 4.0) / p_display_to_lens;
// now we apply our oversample factor to increase our FOV. how much we oversample is always a balance we strike between performance and how much
// we're willing to sacrifice in FOV.
real_t add = ((f1 + f2) * (p_oversample - 1.0)) / 2.0;
f1 += add;
f2 += add;
f3 *= p_oversample;
// always apply KEEP_WIDTH aspect ratio
f3 /= p_aspect;
switch (p_eye) {
case 1: { // left eye
set_frustum(-f2 * p_z_near, f1 * p_z_near, -f3 * p_z_near, f3 * p_z_near, p_z_near, p_z_far);
}; break;
case 2: { // right eye
set_frustum(-f1 * p_z_near, f2 * p_z_near, -f3 * p_z_near, f3 * p_z_near, p_z_near, p_z_far);
}; break;
default: { // mono, does not apply here!
}; break;
};
};
void CameraMatrix::set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar) {
set_identity();
matrix[0][0] = 2.0 / (p_right - p_left);
matrix[3][0] = -((p_right + p_left) / (p_right - p_left));
matrix[1][1] = 2.0 / (p_top - p_bottom);
matrix[3][1] = -((p_top + p_bottom) / (p_top - p_bottom));
matrix[2][2] = -2.0 / (p_zfar - p_znear);
matrix[3][2] = -((p_zfar + p_znear) / (p_zfar - p_znear));
matrix[3][3] = 1.0;
}
void CameraMatrix::set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov) {
if (!p_flip_fov) {
p_size *= p_aspect;
}
set_orthogonal(-p_size / 2, +p_size / 2, -p_size / p_aspect / 2, +p_size / p_aspect / 2, p_znear, p_zfar);
}
void CameraMatrix::set_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far) {
ERR_FAIL_COND(p_right <= p_left);
ERR_FAIL_COND(p_top <= p_bottom);
ERR_FAIL_COND(p_far <= p_near);
real_t *te = &matrix[0][0];
real_t x = 2 * p_near / (p_right - p_left);
real_t y = 2 * p_near / (p_top - p_bottom);
real_t a = (p_right + p_left) / (p_right - p_left);
real_t b = (p_top + p_bottom) / (p_top - p_bottom);
real_t c = -(p_far + p_near) / (p_far - p_near);
real_t d = -2 * p_far * p_near / (p_far - p_near);
te[0] = x;
te[1] = 0;
te[2] = 0;
te[3] = 0;
te[4] = 0;
te[5] = y;
te[6] = 0;
te[7] = 0;
te[8] = a;
te[9] = b;
te[10] = c;
te[11] = -1;
te[12] = 0;
te[13] = 0;
te[14] = d;
te[15] = 0;
}
void CameraMatrix::set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov) {
if (!p_flip_fov) {
p_size *= p_aspect;
}
set_frustum(-p_size / 2 + p_offset.x, +p_size / 2 + p_offset.x, -p_size / p_aspect / 2 + p_offset.y, +p_size / p_aspect / 2 + p_offset.y, p_near, p_far);
}
real_t CameraMatrix::get_z_far() const {
const real_t *matrix = (const real_t *)this->matrix;
Plane new_plane = Plane(matrix[3] - matrix[2],
matrix[7] - matrix[6],
matrix[11] - matrix[10],
matrix[15] - matrix[14]);
new_plane.normal = -new_plane.normal;
new_plane.normalize();
return new_plane.d;
}
real_t CameraMatrix::get_z_near() const {
const real_t *matrix = (const real_t *)this->matrix;
Plane new_plane = Plane(matrix[3] + matrix[2],
matrix[7] + matrix[6],
matrix[11] + matrix[10],
-matrix[15] - matrix[14]);
new_plane.normalize();
return new_plane.d;
}
Vector2 CameraMatrix::get_viewport_half_extents() const {
const real_t *matrix = (const real_t *)this->matrix;
///////--- Near Plane ---///////
Plane near_plane = Plane(matrix[3] + matrix[2],
matrix[7] + matrix[6],
matrix[11] + matrix[10],
-matrix[15] - matrix[14]);
near_plane.normalize();
///////--- Right Plane ---///////
Plane right_plane = Plane(matrix[3] - matrix[0],
matrix[7] - matrix[4],
matrix[11] - matrix[8],
-matrix[15] + matrix[12]);
right_plane.normalize();
Plane top_plane = Plane(matrix[3] - matrix[1],
matrix[7] - matrix[5],
matrix[11] - matrix[9],
-matrix[15] + matrix[13]);
top_plane.normalize();
Vector3 res;
near_plane.intersect_3(right_plane, top_plane, &res);
return Vector2(res.x, res.y);
}
bool CameraMatrix::get_endpoints(const Transform &p_transform, Vector3 *p_8points) const {
Vector<Plane> planes = get_projection_planes(Transform());
const Planes intersections[8][3] = {
{ PLANE_FAR, PLANE_LEFT, PLANE_TOP },
{ PLANE_FAR, PLANE_LEFT, PLANE_BOTTOM },
{ PLANE_FAR, PLANE_RIGHT, PLANE_TOP },
{ PLANE_FAR, PLANE_RIGHT, PLANE_BOTTOM },
{ PLANE_NEAR, PLANE_LEFT, PLANE_TOP },
{ PLANE_NEAR, PLANE_LEFT, PLANE_BOTTOM },
{ PLANE_NEAR, PLANE_RIGHT, PLANE_TOP },
{ PLANE_NEAR, PLANE_RIGHT, PLANE_BOTTOM },
};
for (int i = 0; i < 8; i++) {
Vector3 point;
bool res = planes[intersections[i][0]].intersect_3(planes[intersections[i][1]], planes[intersections[i][2]], &point);
ERR_FAIL_COND_V(!res, false);
p_8points[i] = p_transform.xform(point);
}
return true;
}
Vector<Plane> CameraMatrix::get_projection_planes(const Transform &p_transform) const {
/** Fast Plane Extraction from combined modelview/projection matrices.
* References:
* https://web.archive.org/web/20011221205252/http://www.markmorley.com/opengl/frustumculling.html
* https://web.archive.org/web/20061020020112/http://www2.ravensoft.com/users/ggribb/plane%20extraction.pdf
*/
Vector<Plane> planes;
const real_t *matrix = (const real_t *)this->matrix;
Plane new_plane;
///////--- Near Plane ---///////
new_plane = Plane(matrix[3] + matrix[2],
matrix[7] + matrix[6],
matrix[11] + matrix[10],
matrix[15] + matrix[14]);
new_plane.normal = -new_plane.normal;
new_plane.normalize();
planes.push_back(p_transform.xform(new_plane));
///////--- Far Plane ---///////
new_plane = Plane(matrix[3] - matrix[2],
matrix[7] - matrix[6],
matrix[11] - matrix[10],
matrix[15] - matrix[14]);
new_plane.normal = -new_plane.normal;
new_plane.normalize();
planes.push_back(p_transform.xform(new_plane));
///////--- Left Plane ---///////
new_plane = Plane(matrix[3] + matrix[0],
matrix[7] + matrix[4],
matrix[11] + matrix[8],
matrix[15] + matrix[12]);
new_plane.normal = -new_plane.normal;
new_plane.normalize();
planes.push_back(p_transform.xform(new_plane));
///////--- Top Plane ---///////
new_plane = Plane(matrix[3] - matrix[1],
matrix[7] - matrix[5],
matrix[11] - matrix[9],
matrix[15] - matrix[13]);
new_plane.normal = -new_plane.normal;
new_plane.normalize();
planes.push_back(p_transform.xform(new_plane));
///////--- Right Plane ---///////
new_plane = Plane(matrix[3] - matrix[0],
matrix[7] - matrix[4],
matrix[11] - matrix[8],
matrix[15] - matrix[12]);
new_plane.normal = -new_plane.normal;
new_plane.normalize();
planes.push_back(p_transform.xform(new_plane));
///////--- Bottom Plane ---///////
new_plane = Plane(matrix[3] + matrix[1],
matrix[7] + matrix[5],
matrix[11] + matrix[9],
matrix[15] + matrix[13]);
new_plane.normal = -new_plane.normal;
new_plane.normalize();
planes.push_back(p_transform.xform(new_plane));
return planes;
}
CameraMatrix CameraMatrix::inverse() const {
CameraMatrix cm = *this;
cm.invert();
return cm;
}
void CameraMatrix::invert() {
int i, j, k;
int pvt_i[4], pvt_j[4]; /* Locations of pivot matrix */
real_t pvt_val; /* Value of current pivot element */
real_t hold; /* Temporary storage */
real_t determinat; /* Determinant */
determinat = 1.0;
for (k = 0; k < 4; k++) {
/** Locate k'th pivot element **/
pvt_val = matrix[k][k]; /** Initialize for search **/
pvt_i[k] = k;
pvt_j[k] = k;
for (i = k; i < 4; i++) {
for (j = k; j < 4; j++) {
if (Math::abs(matrix[i][j]) > Math::abs(pvt_val)) {
pvt_i[k] = i;
pvt_j[k] = j;
pvt_val = matrix[i][j];
}
}
}
/** Product of pivots, gives determinant when finished **/
determinat *= pvt_val;
if (Math::abs(determinat) < 1e-7) {
return; //(false); /** Matrix is singular (zero determinant). **/
}
/** "Interchange" rows (with sign change stuff) **/
i = pvt_i[k];
if (i != k) { /** If rows are different **/
for (j = 0; j < 4; j++) {
hold = -matrix[k][j];
matrix[k][j] = matrix[i][j];
matrix[i][j] = hold;
}
}
/** "Interchange" columns **/
j = pvt_j[k];
if (j != k) { /** If columns are different **/
for (i = 0; i < 4; i++) {
hold = -matrix[i][k];
matrix[i][k] = matrix[i][j];
matrix[i][j] = hold;
}
}
/** Divide column by minus pivot value **/
for (i = 0; i < 4; i++) {
if (i != k) {
matrix[i][k] /= (-pvt_val);
}
}
/** Reduce the matrix **/
for (i = 0; i < 4; i++) {
hold = matrix[i][k];
for (j = 0; j < 4; j++) {
if (i != k && j != k) {
matrix[i][j] += hold * matrix[k][j];
}
}
}
/** Divide row by pivot **/
for (j = 0; j < 4; j++) {
if (j != k) {
matrix[k][j] /= pvt_val;
}
}
/** Replace pivot by reciprocal (at last we can touch it). **/
matrix[k][k] = 1.0 / pvt_val;
}
/* That was most of the work, one final pass of row/column interchange */
/* to finish */
for (k = 4 - 2; k >= 0; k--) { /* Don't need to work with 1 by 1 corner*/
i = pvt_j[k]; /* Rows to swap correspond to pivot COLUMN */
if (i != k) { /* If rows are different */
for (j = 0; j < 4; j++) {
hold = matrix[k][j];
matrix[k][j] = -matrix[i][j];
matrix[i][j] = hold;
}
}
j = pvt_i[k]; /* Columns to swap correspond to pivot ROW */
if (j != k) { /* If columns are different */
for (i = 0; i < 4; i++) {
hold = matrix[i][k];
matrix[i][k] = -matrix[i][j];
matrix[i][j] = hold;
}
}
}
}
CameraMatrix::CameraMatrix() {
set_identity();
}
CameraMatrix CameraMatrix::operator*(const CameraMatrix &p_matrix) const {
CameraMatrix new_matrix;
for (int j = 0; j < 4; j++) {
for (int i = 0; i < 4; i++) {
real_t ab = 0;
for (int k = 0; k < 4; k++) {
ab += matrix[k][i] * p_matrix.matrix[j][k];
}
new_matrix.matrix[j][i] = ab;
}
}
return new_matrix;
}
void CameraMatrix::set_light_bias() {
real_t *m = &matrix[0][0];
m[0] = 0.5;
m[1] = 0.0;
m[2] = 0.0;
m[3] = 0.0;
m[4] = 0.0;
m[5] = 0.5;
m[6] = 0.0;
m[7] = 0.0;
m[8] = 0.0;
m[9] = 0.0;
m[10] = 0.5;
m[11] = 0.0;
m[12] = 0.5;
m[13] = 0.5;
m[14] = 0.5;
m[15] = 1.0;
}
void CameraMatrix::set_light_atlas_rect(const Rect2 &p_rect) {
real_t *m = &matrix[0][0];
m[0] = p_rect.w;
m[1] = 0.0;
m[2] = 0.0;
m[3] = 0.0;
m[4] = 0.0;
m[5] = p_rect.h;
m[6] = 0.0;
m[7] = 0.0;
m[8] = 0.0;
m[9] = 0.0;
m[10] = 1.0;
m[11] = 0.0;
m[12] = p_rect.x;
m[13] = p_rect.y;
m[14] = 0.0;
m[15] = 1.0;
}
CameraMatrix::operator String() const {
String str;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
str += String((j > 0) ? ", " : "\n") + String::num(matrix[i][j]);
}
}
return str;
}
real_t CameraMatrix::get_aspect() const {
Vector2 vp_he = get_viewport_half_extents();
return vp_he.x / vp_he.y;
}
int CameraMatrix::get_pixels_per_meter(int p_for_pixel_width) const {
Vector3 result = xform(Vector3(1, 0, -1));
return int((result.x * 0.5 + 0.5) * p_for_pixel_width);
}
bool CameraMatrix::is_orthogonal() const {
return matrix[3][3] == 1.0;
}
real_t CameraMatrix::get_fov() const {
const real_t *matrix = (const real_t *)this->matrix;
Plane right_plane = Plane(matrix[3] - matrix[0],
matrix[7] - matrix[4],
matrix[11] - matrix[8],
-matrix[15] + matrix[12]);
right_plane.normalize();
if ((matrix[8] == 0) && (matrix[9] == 0)) {
return Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x))) * 2.0;
} else {
// our frustum is asymmetrical need to calculate the left planes angle separately..
Plane left_plane = Plane(matrix[3] + matrix[0],
matrix[7] + matrix[4],
matrix[11] + matrix[8],
matrix[15] + matrix[12]);
left_plane.normalize();
return Math::rad2deg(Math::acos(Math::abs(left_plane.normal.x))) + Math::rad2deg(Math::acos(Math::abs(right_plane.normal.x)));
}
}
void CameraMatrix::make_scale(const Vector3 &p_scale) {
set_identity();
matrix[0][0] = p_scale.x;
matrix[1][1] = p_scale.y;
matrix[2][2] = p_scale.z;
}
void CameraMatrix::scale_translate_to_fit(const AABB &p_aabb) {
Vector3 min = p_aabb.position;
Vector3 max = p_aabb.position + p_aabb.size;
matrix[0][0] = 2 / (max.x - min.x);
matrix[1][0] = 0;
matrix[2][0] = 0;
matrix[3][0] = -(max.x + min.x) / (max.x - min.x);
matrix[0][1] = 0;
matrix[1][1] = 2 / (max.y - min.y);
matrix[2][1] = 0;
matrix[3][1] = -(max.y + min.y) / (max.y - min.y);
matrix[0][2] = 0;
matrix[1][2] = 0;
matrix[2][2] = 2 / (max.z - min.z);
matrix[3][2] = -(max.z + min.z) / (max.z - min.z);
matrix[0][3] = 0;
matrix[1][3] = 0;
matrix[2][3] = 0;
matrix[3][3] = 1;
}
CameraMatrix::operator Transform() const {
Transform tr;
const real_t *m = &matrix[0][0];
tr.basis.elements[0][0] = m[0];
tr.basis.elements[1][0] = m[1];
tr.basis.elements[2][0] = m[2];
tr.basis.elements[0][1] = m[4];
tr.basis.elements[1][1] = m[5];
tr.basis.elements[2][1] = m[6];
tr.basis.elements[0][2] = m[8];
tr.basis.elements[1][2] = m[9];
tr.basis.elements[2][2] = m[10];
tr.origin.x = m[12];
tr.origin.y = m[13];
tr.origin.z = m[14];
return tr;
}
CameraMatrix::CameraMatrix(const Transform &p_transform) {
const Transform &tr = p_transform;
real_t *m = &matrix[0][0];
m[0] = tr.basis.elements[0][0];
m[1] = tr.basis.elements[1][0];
m[2] = tr.basis.elements[2][0];
m[3] = 0.0;
m[4] = tr.basis.elements[0][1];
m[5] = tr.basis.elements[1][1];
m[6] = tr.basis.elements[2][1];
m[7] = 0.0;
m[8] = tr.basis.elements[0][2];
m[9] = tr.basis.elements[1][2];
m[10] = tr.basis.elements[2][2];
m[11] = 0.0;
m[12] = tr.origin.x;
m[13] = tr.origin.y;
m[14] = tr.origin.z;
m[15] = 1.0;
}
CameraMatrix::~CameraMatrix() {
}

106
core/math/camera_matrix.h Normal file
View File

@ -0,0 +1,106 @@
/*************************************************************************/
/* camera_matrix.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef CAMERA_MATRIX_H
#define CAMERA_MATRIX_H
#include "core/math/rect2.h"
#include "core/math/transform.h"
#include "core/math/vector2.h"
struct CameraMatrix {
enum Planes {
PLANE_NEAR,
PLANE_FAR,
PLANE_LEFT,
PLANE_TOP,
PLANE_RIGHT,
PLANE_BOTTOM
};
real_t matrix[4][4];
void set_identity();
void set_zero();
void set_light_bias();
void set_light_atlas_rect(const Rect2 &p_rect);
void set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov = false);
void set_perspective(real_t p_fovy_degrees, real_t p_aspect, real_t p_z_near, real_t p_z_far, bool p_flip_fov, int p_eye, real_t p_intraocular_dist, real_t p_convergence_dist);
void set_for_hmd(int p_eye, real_t p_aspect, real_t p_intraocular_dist, real_t p_display_width, real_t p_display_to_lens, real_t p_oversample, real_t p_z_near, real_t p_z_far);
void set_orthogonal(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_znear, real_t p_zfar);
void set_orthogonal(real_t p_size, real_t p_aspect, real_t p_znear, real_t p_zfar, bool p_flip_fov = false);
void set_frustum(real_t p_left, real_t p_right, real_t p_bottom, real_t p_top, real_t p_near, real_t p_far);
void set_frustum(real_t p_size, real_t p_aspect, Vector2 p_offset, real_t p_near, real_t p_far, bool p_flip_fov = false);
static real_t get_fovy(real_t p_fovx, real_t p_aspect) {
return Math::rad2deg(Math::atan(p_aspect * Math::tan(Math::deg2rad(p_fovx) * 0.5)) * 2.0);
}
real_t get_z_far() const;
real_t get_z_near() const;
real_t get_aspect() const;
real_t get_fov() const;
bool is_orthogonal() const;
Vector<Plane> get_projection_planes(const Transform &p_transform) const;
bool get_endpoints(const Transform &p_transform, Vector3 *p_8points) const;
Vector2 get_viewport_half_extents() const;
void invert();
CameraMatrix inverse() const;
CameraMatrix operator*(const CameraMatrix &p_matrix) const;
Plane xform4(const Plane &p_vec4) const;
_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vec3) const;
operator String() const;
void scale_translate_to_fit(const AABB &p_aabb);
void make_scale(const Vector3 &p_scale);
int get_pixels_per_meter(int p_for_pixel_width) const;
operator Transform() const;
CameraMatrix();
CameraMatrix(const Transform &p_transform);
~CameraMatrix();
};
Vector3 CameraMatrix::xform(const Vector3 &p_vec3) const {
Vector3 ret;
ret.x = matrix[0][0] * p_vec3.x + matrix[1][0] * p_vec3.y + matrix[2][0] * p_vec3.z + matrix[3][0];
ret.y = matrix[0][1] * p_vec3.x + matrix[1][1] * p_vec3.y + matrix[2][1] * p_vec3.z + matrix[3][1];
ret.z = matrix[0][2] * p_vec3.x + matrix[1][2] * p_vec3.y + matrix[2][2] * p_vec3.z + matrix[3][2];
real_t w = matrix[0][3] * p_vec3.x + matrix[1][3] * p_vec3.y + matrix[2][3] * p_vec3.z + matrix[3][3];
return ret / w;
}
#endif

115
core/math/math_defs.h Normal file
View File

@ -0,0 +1,115 @@
/*************************************************************************/
/* math_defs.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef MATH_DEFS_H
#define MATH_DEFS_H
#define CMP_EPSILON 0.00001
#define CMP_EPSILON2 (CMP_EPSILON * CMP_EPSILON)
#define CMP_NORMALIZE_TOLERANCE 0.000001
#define CMP_POINT_IN_PLANE_EPSILON 0.00001
#define Math_SQRT12 0.7071067811865475244008443621048490
#define Math_SQRT2 1.4142135623730950488016887242
#define Math_LN2 0.6931471805599453094172321215
#define Math_TAU 6.2831853071795864769252867666
#define Math_PI 3.1415926535897932384626433833
#define Math_E 2.7182818284590452353602874714
#define Math_INF INFINITY
#define Math_NAN NAN
#ifdef DEBUG_ENABLED
//#define MATH_CHECKS
#endif
//this epsilon is for values related to a unit size (scalar or vector len)
#ifdef PRECISE_MATH_CHECKS
#define UNIT_EPSILON 0.00001
#else
//tolerate some more floating point error normally
#define UNIT_EPSILON 0.001
#endif
#define USEC_TO_SEC(m_usec) ((m_usec) / 1000000.0)
enum ClockDirection {
CLOCKWISE,
COUNTERCLOCKWISE
};
enum Orientation {
HORIZONTAL,
VERTICAL
};
enum HAlign {
HALIGN_LEFT,
HALIGN_CENTER,
HALIGN_RIGHT
};
enum VAlign {
VALIGN_TOP,
VALIGN_CENTER,
VALIGN_BOTTOM
};
enum Margin {
MARGIN_LEFT,
MARGIN_TOP,
MARGIN_RIGHT,
MARGIN_BOTTOM
};
enum Corner {
CORNER_TOP_LEFT,
CORNER_TOP_RIGHT,
CORNER_BOTTOM_RIGHT,
CORNER_BOTTOM_LEFT
};
/**
* The "Real" type is an abstract type used for real numbers, such as 1.5,
* in contrast to integer numbers. Precision can be controlled with the
* presence or absence of the REAL_T_IS_DOUBLE define.
*/
#ifdef REAL_T_IS_DOUBLE
typedef double real_t;
#else
typedef float real_t;
#endif
#endif // MATH_DEFS_H

154
core/math/plane.cpp Normal file
View File

@ -0,0 +1,154 @@
/*************************************************************************/
/* plane.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "plane.h"
#include "core/math/math.h"
#include "core/math/math_defs.h"
void Plane::set_normal(const Vector3 &p_normal) {
normal = p_normal;
}
void Plane::normalize() {
real_t l = normal.length();
if (l == 0) {
*this = Plane(0, 0, 0, 0);
return;
}
normal /= l;
d /= l;
}
Plane Plane::normalized() const {
Plane p = *this;
p.normalize();
return p;
}
Vector3 Plane::get_any_point() const {
return get_normal() * d;
}
Vector3 Plane::get_any_perpendicular_normal() const {
static const Vector3 p1 = Vector3(1, 0, 0);
static const Vector3 p2 = Vector3(0, 1, 0);
Vector3 p;
if (ABS(normal.dot(p1)) > 0.99) { // if too similar to p1
p = p2; // use p2
} else {
p = p1; // use p1
}
p -= normal * normal.dot(p);
p.normalize();
return p;
}
/* intersections */
bool Plane::intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result) const {
const Plane &p_plane0 = *this;
Vector3 normal0 = p_plane0.normal;
Vector3 normal1 = p_plane1.normal;
Vector3 normal2 = p_plane2.normal;
real_t denom = normal0.cross(normal1).dot(normal2);
if (Math::is_zero_approx(denom)) {
return false;
}
if (r_result) {
*r_result = ((normal1.cross(normal2) * p_plane0.d) +
(normal2.cross(normal0) * p_plane1.d) +
(normal0.cross(normal1) * p_plane2.d)) /
denom;
}
return true;
}
bool Plane::intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const {
Vector3 segment = p_dir;
real_t den = normal.dot(segment);
//printf("den is %i\n",den);
if (Math::is_zero_approx(den)) {
return false;
}
real_t dist = (normal.dot(p_from) - d) / den;
//printf("dist is %i\n",dist);
if (dist > CMP_EPSILON) { //this is a ray, before the emitting pos (p_from) doesn't exist
return false;
}
dist = -dist;
*p_intersection = p_from + segment * dist;
return true;
}
bool Plane::intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const {
Vector3 segment = p_begin - p_end;
real_t den = normal.dot(segment);
//printf("den is %i\n",den);
if (Math::is_zero_approx(den)) {
return false;
}
real_t dist = (normal.dot(p_begin) - d) / den;
//printf("dist is %i\n",dist);
if (dist < -CMP_EPSILON || dist > (1.0 + CMP_EPSILON)) {
return false;
}
dist = -dist;
*p_intersection = p_begin + segment * dist;
return true;
}
/* misc */
bool Plane::is_equal_approx(const Plane &p_plane) const {
return normal.is_equal_approx(p_plane.normal) && Math::is_equal_approx(d, p_plane.d);
}
Plane::operator String() const {
return normal.operator String() + ", " + String::num(d);
}

133
core/math/plane.h Normal file
View File

@ -0,0 +1,133 @@
/*************************************************************************/
/* plane.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef PLANE_H
#define PLANE_H
#include "core/typedefs.h"
#include "core/math/vector3.h"
#include "core/string.h"
#include "core/math/math_defs.h"
class Plane {
public:
Vector3 normal;
real_t d;
void set_normal(const Vector3 &p_normal);
_FORCE_INLINE_ Vector3 get_normal() const { return normal; }; ///Point is coplanar, CMP_EPSILON for precision
void normalize();
Plane normalized() const;
/* Plane-Point operations */
_FORCE_INLINE_ Vector3 center() const { return normal * d; }
Vector3 get_any_point() const;
Vector3 get_any_perpendicular_normal() const;
_FORCE_INLINE_ bool is_point_over(const Vector3 &p_point) const; ///< Point is over plane
_FORCE_INLINE_ real_t distance_to(const Vector3 &p_point) const;
_FORCE_INLINE_ bool has_point(const Vector3 &p_point, real_t _epsilon = CMP_EPSILON) const;
/* intersections */
bool intersect_3(const Plane &p_plane1, const Plane &p_plane2, Vector3 *r_result = nullptr) const;
bool intersects_ray(const Vector3 &p_from, const Vector3 &p_dir, Vector3 *p_intersection) const;
bool intersects_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 *p_intersection) const;
_FORCE_INLINE_ Vector3 project(const Vector3 &p_point) const {
return p_point - normal * distance_to(p_point);
}
/* misc */
Plane operator-() const { return Plane(-normal, -d); }
bool is_equal_approx(const Plane &p_plane) const;
_FORCE_INLINE_ bool operator==(const Plane &p_plane) const;
_FORCE_INLINE_ bool operator!=(const Plane &p_plane) const;
operator String() const;
_FORCE_INLINE_ Plane() :
d(0) {}
_FORCE_INLINE_ Plane(real_t p_a, real_t p_b, real_t p_c, real_t p_d) :
normal(p_a, p_b, p_c),
d(p_d) {}
_FORCE_INLINE_ Plane(const Vector3 &p_normal, real_t p_d);
_FORCE_INLINE_ Plane(const Vector3 &p_point, const Vector3 &p_normal);
_FORCE_INLINE_ Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3, ClockDirection p_dir = CLOCKWISE);
};
bool Plane::is_point_over(const Vector3 &p_point) const {
return (normal.dot(p_point) > d);
}
real_t Plane::distance_to(const Vector3 &p_point) const {
return (normal.dot(p_point) - d);
}
bool Plane::has_point(const Vector3 &p_point, real_t _epsilon) const {
real_t dist = normal.dot(p_point) - d;
dist = ABS(dist);
return (dist <= _epsilon);
}
Plane::Plane(const Vector3 &p_normal, real_t p_d) :
normal(p_normal),
d(p_d) {
}
Plane::Plane(const Vector3 &p_point, const Vector3 &p_normal) :
normal(p_normal),
d(p_normal.dot(p_point)) {
}
Plane::Plane(const Vector3 &p_point1, const Vector3 &p_point2, const Vector3 &p_point3, ClockDirection p_dir) {
if (p_dir == CLOCKWISE) {
normal = (p_point1 - p_point3).cross(p_point1 - p_point2);
} else {
normal = (p_point1 - p_point2).cross(p_point1 - p_point3);
}
normal.normalize();
d = normal.dot(p_point1);
}
bool Plane::operator==(const Plane &p_plane) const {
return normal == p_plane.normal && d == p_plane.d;
}
bool Plane::operator!=(const Plane &p_plane) const {
return normal != p_plane.normal || d != p_plane.d;
}
#endif // PLANE_H

254
core/math/quat.cpp Normal file
View File

@ -0,0 +1,254 @@
/*************************************************************************/
/* quat.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "quat.h"
#include "core/math/basis.h"
real_t Quat::angle_to(const Quat &p_to) const {
real_t d = dot(p_to);
return Math::acos(CLAMP(d * d * 2 - 1, -1, 1));
}
// set_euler_xyz expects a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// This implementation uses XYZ convention (Z is the first rotation).
void Quat::set_euler_xyz(const Vector3 &p_euler) {
real_t half_a1 = p_euler.x * 0.5;
real_t half_a2 = p_euler.y * 0.5;
real_t half_a3 = p_euler.z * 0.5;
// R = X(a1).Y(a2).Z(a3) convention for Euler angles.
// Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-2)
// a3 is the angle of the first rotation, following the notation in this reference.
real_t cos_a1 = Math::cos(half_a1);
real_t sin_a1 = Math::sin(half_a1);
real_t cos_a2 = Math::cos(half_a2);
real_t sin_a2 = Math::sin(half_a2);
real_t cos_a3 = Math::cos(half_a3);
real_t sin_a3 = Math::sin(half_a3);
set(sin_a1 * cos_a2 * cos_a3 + sin_a2 * sin_a3 * cos_a1,
-sin_a1 * sin_a3 * cos_a2 + sin_a2 * cos_a1 * cos_a3,
sin_a1 * sin_a2 * cos_a3 + sin_a3 * cos_a1 * cos_a2,
-sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3);
}
// get_euler_xyz returns a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// This implementation uses XYZ convention (Z is the first rotation).
Vector3 Quat::get_euler_xyz() const {
Basis m(*this);
return m.get_euler_xyz();
}
// set_euler_yxz expects a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// This implementation uses YXZ convention (Z is the first rotation).
void Quat::set_euler_yxz(const Vector3 &p_euler) {
real_t half_a1 = p_euler.y * 0.5;
real_t half_a2 = p_euler.x * 0.5;
real_t half_a3 = p_euler.z * 0.5;
// R = Y(a1).X(a2).Z(a3) convention for Euler angles.
// Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6)
// a3 is the angle of the first rotation, following the notation in this reference.
real_t cos_a1 = Math::cos(half_a1);
real_t sin_a1 = Math::sin(half_a1);
real_t cos_a2 = Math::cos(half_a2);
real_t sin_a2 = Math::sin(half_a2);
real_t cos_a3 = Math::cos(half_a3);
real_t sin_a3 = Math::sin(half_a3);
set(sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3,
sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3,
-sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3,
sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3);
}
// get_euler_yxz returns a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// This implementation uses YXZ convention (Z is the first rotation).
Vector3 Quat::get_euler_yxz() const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized.");
#endif
Basis m(*this);
return m.get_euler_yxz();
}
void Quat::operator*=(const Quat &p_q) {
set(w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y,
w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z,
w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x,
w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z);
}
Quat Quat::operator*(const Quat &p_q) const {
Quat r = *this;
r *= p_q;
return r;
}
bool Quat::is_equal_approx(const Quat &p_quat) const {
return Math::is_equal_approx(x, p_quat.x) && Math::is_equal_approx(y, p_quat.y) && Math::is_equal_approx(z, p_quat.z) && Math::is_equal_approx(w, p_quat.w);
}
real_t Quat::length() const {
return Math::sqrt(length_squared());
}
void Quat::normalize() {
*this /= length();
}
Quat Quat::normalized() const {
return *this / length();
}
bool Quat::is_normalized() const {
return Math::is_equal_approx(length_squared(), 1);//, (real_t)UNIT_EPSILON); //use less epsilon
}
Quat Quat::inverse() const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The quaternion must be normalized.");
#endif
return Quat(-x, -y, -z, w);
}
Quat Quat::slerp(const Quat &p_to, const real_t &p_weight) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quat(), "The end quaternion must be normalized.");
#endif
Quat to1;
real_t omega, cosom, sinom, scale0, scale1;
// calc cosine
cosom = dot(p_to);
// adjust signs (if necessary)
if (cosom < 0.0) {
cosom = -cosom;
to1.x = -p_to.x;
to1.y = -p_to.y;
to1.z = -p_to.z;
to1.w = -p_to.w;
} else {
to1.x = p_to.x;
to1.y = p_to.y;
to1.z = p_to.z;
to1.w = p_to.w;
}
// calculate coefficients
if ((1.0 - cosom) > CMP_EPSILON) {
// standard case (slerp)
omega = Math::acos(cosom);
sinom = Math::sin(omega);
scale0 = Math::sin((1.0 - p_weight) * omega) / sinom;
scale1 = Math::sin(p_weight * omega) / sinom;
} else {
// "from" and "to" quaternions are very close
// ... so we can do a linear interpolation
scale0 = 1.0 - p_weight;
scale1 = p_weight;
}
// calculate final values
return Quat(
scale0 * x + scale1 * to1.x,
scale0 * y + scale1 * to1.y,
scale0 * z + scale1 * to1.z,
scale0 * w + scale1 * to1.w);
}
Quat Quat::slerpni(const Quat &p_to, const real_t &p_weight) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quat(), "The end quaternion must be normalized.");
#endif
const Quat &from = *this;
real_t dot = from.dot(p_to);
if (Math::abs(dot) > 0.9999) {
return from;
}
real_t theta = Math::acos(dot),
sinT = 1.0 / Math::sin(theta),
newFactor = Math::sin(p_weight * theta) * sinT,
invFactor = Math::sin((1.0 - p_weight) * theta) * sinT;
return Quat(invFactor * from.x + newFactor * p_to.x,
invFactor * from.y + newFactor * p_to.y,
invFactor * from.z + newFactor * p_to.z,
invFactor * from.w + newFactor * p_to.w);
}
Quat Quat::cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), Quat(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quat(), "The end quaternion must be normalized.");
#endif
//the only way to do slerp :|
real_t t2 = (1.0 - p_weight) * p_weight * 2;
Quat sp = this->slerp(p_b, p_weight);
Quat sq = p_pre_a.slerpni(p_post_b, p_weight);
return sp.slerpni(sq, t2);
}
Quat::operator String() const {
return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w);
}
void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) {
#ifdef MATH_CHECKS
ERR_FAIL_COND_MSG(!axis.is_normalized(), "The axis Vector3 must be normalized.");
#endif
real_t d = axis.length();
if (d == 0) {
set(0, 0, 0, 0);
} else {
real_t sin_angle = Math::sin(angle * 0.5);
real_t cos_angle = Math::cos(angle * 0.5);
real_t s = sin_angle / d;
set(axis.x * s, axis.y * s, axis.z * s,
cos_angle);
}
}

232
core/math/quat.h Normal file
View File

@ -0,0 +1,232 @@
/*************************************************************************/
/* quat.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef QUAT_H
#define QUAT_H
#include "core/math/math_defs.h"
#include "core/math/math.h"
#include "core/math/vector3.h"
#include "core/string.h"
#include "core/typedefs.h"
class Quat {
public:
real_t x, y, z, w;
_FORCE_INLINE_ real_t length_squared() const;
bool is_equal_approx(const Quat &p_quat) const;
real_t length() const;
void normalize();
Quat normalized() const;
bool is_normalized() const;
Quat inverse() const;
_FORCE_INLINE_ real_t dot(const Quat &p_q) const;
real_t angle_to(const Quat &p_to) const;
void set_euler_xyz(const Vector3 &p_euler);
Vector3 get_euler_xyz() const;
void set_euler_yxz(const Vector3 &p_euler);
Vector3 get_euler_yxz() const;
void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); };
Vector3 get_euler() const { return get_euler_yxz(); };
Quat slerp(const Quat &p_to, const real_t &p_weight) const;
Quat slerpni(const Quat &p_to, const real_t &p_weight) const;
Quat cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) const;
void set_axis_angle(const Vector3 &axis, const real_t &angle);
_FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
r_angle = 2 * Math::acos(w);
real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
r_axis.x = x * r;
r_axis.y = y * r;
r_axis.z = z * r;
}
void operator*=(const Quat &p_q);
Quat operator*(const Quat &p_q) const;
Quat operator*(const Vector3 &v) const {
return Quat(w * v.x + y * v.z - z * v.y,
w * v.y + z * v.x - x * v.z,
w * v.z + x * v.y - y * v.x,
-x * v.x - y * v.y - z * v.z);
}
_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized.");
#endif
Vector3 u(x, y, z);
Vector3 uv = u.cross(v);
return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
}
_FORCE_INLINE_ void operator+=(const Quat &p_q);
_FORCE_INLINE_ void operator-=(const Quat &p_q);
_FORCE_INLINE_ void operator*=(const real_t &s);
_FORCE_INLINE_ void operator/=(const real_t &s);
_FORCE_INLINE_ Quat operator+(const Quat &q2) const;
_FORCE_INLINE_ Quat operator-(const Quat &q2) const;
_FORCE_INLINE_ Quat operator-() const;
_FORCE_INLINE_ Quat operator*(const real_t &s) const;
_FORCE_INLINE_ Quat operator/(const real_t &s) const;
_FORCE_INLINE_ bool operator==(const Quat &p_quat) const;
_FORCE_INLINE_ bool operator!=(const Quat &p_quat) const;
operator String() const;
inline void set(real_t p_x, real_t p_y, real_t p_z, real_t p_w) {
x = p_x;
y = p_y;
z = p_z;
w = p_w;
}
inline Quat(real_t p_x, real_t p_y, real_t p_z, real_t p_w) :
x(p_x),
y(p_y),
z(p_z),
w(p_w) {
}
Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); }
Quat(const Vector3 &euler) { set_euler(euler); }
Quat(const Quat &p_q) :
x(p_q.x),
y(p_q.y),
z(p_q.z),
w(p_q.w) {
}
Quat operator=(const Quat &p_q) {
x = p_q.x;
y = p_q.y;
z = p_q.z;
w = p_q.w;
return *this;
}
Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc
{
Vector3 c = v0.cross(v1);
real_t d = v0.dot(v1);
if (d < -1.0 + CMP_EPSILON) {
x = 0;
y = 1;
z = 0;
w = 0;
} else {
real_t s = Math::sqrt((1.0 + d) * 2.0);
real_t rs = 1.0 / s;
x = c.x * rs;
y = c.y * rs;
z = c.z * rs;
w = s * 0.5;
}
}
inline Quat() :
x(0),
y(0),
z(0),
w(1) {
}
};
real_t Quat::dot(const Quat &p_q) const {
return x * p_q.x + y * p_q.y + z * p_q.z + w * p_q.w;
}
real_t Quat::length_squared() const {
return dot(*this);
}
void Quat::operator+=(const Quat &p_q) {
x += p_q.x;
y += p_q.y;
z += p_q.z;
w += p_q.w;
}
void Quat::operator-=(const Quat &p_q) {
x -= p_q.x;
y -= p_q.y;
z -= p_q.z;
w -= p_q.w;
}
void Quat::operator*=(const real_t &s) {
x *= s;
y *= s;
z *= s;
w *= s;
}
void Quat::operator/=(const real_t &s) {
*this *= 1.0 / s;
}
Quat Quat::operator+(const Quat &q2) const {
const Quat &q1 = *this;
return Quat(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w);
}
Quat Quat::operator-(const Quat &q2) const {
const Quat &q1 = *this;
return Quat(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w);
}
Quat Quat::operator-() const {
const Quat &q2 = *this;
return Quat(-q2.x, -q2.y, -q2.z, -q2.w);
}
Quat Quat::operator*(const real_t &s) const {
return Quat(x * s, y * s, z * s, w * s);
}
Quat Quat::operator/(const real_t &s) const {
return *this * (1.0 / s);
}
bool Quat::operator==(const Quat &p_quat) const {
return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w;
}
bool Quat::operator!=(const Quat &p_quat) const {
return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w;
}
#endif

202
core/math/transform.cpp Normal file
View File

@ -0,0 +1,202 @@
/*************************************************************************/
/* transform.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "transform.h"
#include "core/math/math.h"
void Transform::affine_invert() {
basis.invert();
origin = basis.xform(-origin);
}
Transform Transform::affine_inverse() const {
Transform ret = *this;
ret.affine_invert();
return ret;
}
void Transform::invert() {
basis.transpose();
origin = basis.xform(-origin);
}
Transform Transform::inverse() const {
// FIXME: this function assumes the basis is a rotation matrix, with no scaling.
// Transform::affine_inverse can handle matrices with scaling, so GDScript should eventually use that.
Transform ret = *this;
ret.invert();
return ret;
}
void Transform::rotate(const Vector3 &p_axis, real_t p_phi) {
*this = rotated(p_axis, p_phi);
}
Transform Transform::rotated(const Vector3 &p_axis, real_t p_phi) const {
return Transform(Basis(p_axis, p_phi), Vector3()) * (*this);
}
void Transform::rotate_basis(const Vector3 &p_axis, real_t p_phi) {
basis.rotate(p_axis, p_phi);
}
Transform Transform::looking_at(const Vector3 &p_target, const Vector3 &p_up) const {
Transform t = *this;
t.set_look_at(origin, p_target, p_up);
return t;
}
void Transform::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) {
#ifdef MATH_CHECKS
ERR_FAIL_COND(p_eye == p_target);
ERR_FAIL_COND(p_up.length() == 0);
#endif
// Reference: MESA source code
Vector3 v_x, v_y, v_z;
/* Make rotation matrix */
/* Z vector */
v_z = p_eye - p_target;
v_z.normalize();
v_y = p_up;
v_x = v_y.cross(v_z);
#ifdef MATH_CHECKS
ERR_FAIL_COND(v_x.length() == 0);
#endif
/* Recompute Y = Z cross X */
v_y = v_z.cross(v_x);
v_x.normalize();
v_y.normalize();
basis.set(v_x, v_y, v_z);
origin = p_eye;
}
Transform Transform::interpolate_with(const Transform &p_transform, real_t p_c) const {
/* not sure if very "efficient" but good enough? */
Vector3 src_scale = basis.get_scale();
Quat src_rot = basis.get_rotation_quat();
Vector3 src_loc = origin;
Vector3 dst_scale = p_transform.basis.get_scale();
Quat dst_rot = p_transform.basis.get_rotation_quat();
Vector3 dst_loc = p_transform.origin;
Transform interp;
interp.basis.set_quat_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c));
interp.origin = src_loc.lerp(dst_loc, p_c);
return interp;
}
void Transform::scale(const Vector3 &p_scale) {
basis.scale(p_scale);
origin *= p_scale;
}
Transform Transform::scaled(const Vector3 &p_scale) const {
Transform t = *this;
t.scale(p_scale);
return t;
}
void Transform::scale_basis(const Vector3 &p_scale) {
basis.scale(p_scale);
}
void Transform::translate(real_t p_tx, real_t p_ty, real_t p_tz) {
translate(Vector3(p_tx, p_ty, p_tz));
}
void Transform::translate(const Vector3 &p_translation) {
for (int i = 0; i < 3; i++) {
origin[i] += basis[i].dot(p_translation);
}
}
Transform Transform::translated(const Vector3 &p_translation) const {
Transform t = *this;
t.translate(p_translation);
return t;
}
void Transform::orthonormalize() {
basis.orthonormalize();
}
Transform Transform::orthonormalized() const {
Transform _copy = *this;
_copy.orthonormalize();
return _copy;
}
bool Transform::is_equal_approx(const Transform &p_transform) const {
return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin);
}
bool Transform::operator==(const Transform &p_transform) const {
return (basis == p_transform.basis && origin == p_transform.origin);
}
bool Transform::operator!=(const Transform &p_transform) const {
return (basis != p_transform.basis || origin != p_transform.origin);
}
void Transform::operator*=(const Transform &p_transform) {
origin = xform(p_transform.origin);
basis *= p_transform.basis;
}
Transform Transform::operator*(const Transform &p_transform) const {
Transform t = *this;
t *= p_transform;
return t;
}
Transform::operator String() const {
return basis.operator String() + " - " + origin.operator String();
}
Transform::Transform(const Basis &p_basis, const Vector3 &p_origin) :
basis(p_basis),
origin(p_origin) {
}
Transform::Transform(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) {
basis = Basis(xx, xy, xz, yx, yy, yz, zx, zy, zz);
origin = Vector3(ox, oy, oz);
}

257
core/math/transform.h Normal file
View File

@ -0,0 +1,257 @@
/*************************************************************************/
/* transform.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef TRANSFORM_H
#define TRANSFORM_H
#include "core/math/aabb.h"
#include "core/math/basis.h"
#include "core/math/plane.h"
#include "core/containers/vector.h"
class Transform {
public:
Basis basis;
Vector3 origin;
void invert();
Transform inverse() const;
void affine_invert();
Transform affine_inverse() const;
Transform rotated(const Vector3 &p_axis, real_t p_phi) const;
void rotate(const Vector3 &p_axis, real_t p_phi);
void rotate_basis(const Vector3 &p_axis, real_t p_phi);
void set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up);
Transform looking_at(const Vector3 &p_target, const Vector3 &p_up) const;
void scale(const Vector3 &p_scale);
Transform scaled(const Vector3 &p_scale) const;
void scale_basis(const Vector3 &p_scale);
void translate(real_t p_tx, real_t p_ty, real_t p_tz);
void translate(const Vector3 &p_translation);
Transform translated(const Vector3 &p_translation) const;
const Basis &get_basis() const { return basis; }
void set_basis(const Basis &p_basis) { basis = p_basis; }
const Vector3 &get_origin() const { return origin; }
void set_origin(const Vector3 &p_origin) { origin = p_origin; }
void orthonormalize();
Transform orthonormalized() const;
bool is_equal_approx(const Transform &p_transform) const;
bool operator==(const Transform &p_transform) const;
bool operator!=(const Transform &p_transform) const;
_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
_FORCE_INLINE_ AABB xform(const AABB &p_aabb) const;
_FORCE_INLINE_ Vector<Vector3> xform(const Vector<Vector3> &p_array) const;
// NOTE: These are UNSAFE with non-uniform scaling, and will produce incorrect results.
// They use the transpose.
// For safe inverse transforms, xform by the affine_inverse.
_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
_FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const;
_FORCE_INLINE_ Vector<Vector3> xform_inv(const Vector<Vector3> &p_array) const;
// Safe with non-uniform scaling (uses affine_inverse).
_FORCE_INLINE_ Plane xform(const Plane &p_plane) const;
_FORCE_INLINE_ Plane xform_inv(const Plane &p_plane) const;
// These fast versions use precomputed affine inverse, and should be used in bottleneck areas where
// multiple planes are to be transformed.
_FORCE_INLINE_ Plane xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const;
static _FORCE_INLINE_ Plane xform_inv_fast(const Plane &p_plane, const Transform &p_inverse, const Basis &p_basis_transpose);
void operator*=(const Transform &p_transform);
Transform operator*(const Transform &p_transform) const;
Transform interpolate_with(const Transform &p_transform, real_t p_c) const;
_FORCE_INLINE_ Transform inverse_xform(const Transform &t) const {
Vector3 v = t.origin - origin;
return Transform(basis.transpose_xform(t.basis),
basis.xform(v));
}
void set(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 tx, real_t ty, real_t tz) {
basis.set(xx, xy, xz, yx, yy, yz, zx, zy, zz);
origin.x = tx;
origin.y = ty;
origin.z = tz;
}
operator String() const;
Transform(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);
Transform(const Basis &p_basis, const Vector3 &p_origin = Vector3());
Transform() {}
};
_FORCE_INLINE_ Vector3 Transform::xform(const Vector3 &p_vector) const {
return Vector3(
basis[0].dot(p_vector) + origin.x,
basis[1].dot(p_vector) + origin.y,
basis[2].dot(p_vector) + origin.z);
}
_FORCE_INLINE_ Vector3 Transform::xform_inv(const Vector3 &p_vector) const {
Vector3 v = p_vector - origin;
return Vector3(
(basis.elements[0][0] * v.x) + (basis.elements[1][0] * v.y) + (basis.elements[2][0] * v.z),
(basis.elements[0][1] * v.x) + (basis.elements[1][1] * v.y) + (basis.elements[2][1] * v.z),
(basis.elements[0][2] * v.x) + (basis.elements[1][2] * v.y) + (basis.elements[2][2] * v.z));
}
// Neither the plane regular xform or xform_inv are particularly efficient,
// as they do a basis inverse. For xforming a large number
// of planes it is better to pre-calculate the inverse transpose basis once
// and reuse it for each plane, by using the 'fast' version of the functions.
_FORCE_INLINE_ Plane Transform::xform(const Plane &p_plane) const {
Basis b = basis.inverse();
b.transpose();
return xform_fast(p_plane, b);
}
_FORCE_INLINE_ Plane Transform::xform_inv(const Plane &p_plane) const {
Transform inv = affine_inverse();
Basis basis_transpose = basis.transposed();
return xform_inv_fast(p_plane, inv, basis_transpose);
}
_FORCE_INLINE_ AABB Transform::xform(const AABB &p_aabb) const {
/* http://dev.theomader.com/transform-bounding-boxes/ */
Vector3 min = p_aabb.position;
Vector3 max = p_aabb.position + p_aabb.size;
Vector3 tmin, tmax;
for (int i = 0; i < 3; i++) {
tmin[i] = tmax[i] = origin[i];
for (int j = 0; j < 3; j++) {
real_t e = basis[i][j] * min[j];
real_t f = basis[i][j] * max[j];
if (e < f) {
tmin[i] += e;
tmax[i] += f;
} else {
tmin[i] += f;
tmax[i] += e;
}
}
}
AABB r_aabb;
r_aabb.position = tmin;
r_aabb.size = tmax - tmin;
return r_aabb;
}
_FORCE_INLINE_ AABB Transform::xform_inv(const AABB &p_aabb) const {
/* define vertices */
Vector3 vertices[8] = {
Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z),
Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z),
Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z),
Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y, p_aabb.position.z),
Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z),
Vector3(p_aabb.position.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z),
Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z + p_aabb.size.z),
Vector3(p_aabb.position.x, p_aabb.position.y, p_aabb.position.z)
};
AABB ret;
ret.position = xform_inv(vertices[0]);
for (int i = 1; i < 8; i++) {
ret.expand_to(xform_inv(vertices[i]));
}
return ret;
}
Vector<Vector3> Transform::xform(const Vector<Vector3> &p_array) const {
Vector<Vector3> array;
array.resize(p_array.size());
for (int i = 0; i < p_array.size(); ++i) {
array[i] = xform(p_array[i]);
}
return array;
}
Vector<Vector3> Transform::xform_inv(const Vector<Vector3> &p_array) const {
Vector<Vector3> array;
array.resize(p_array.size());
for (int i = 0; i < p_array.size(); ++i) {
array[i] = xform_inv(p_array[i]);
}
return array;
}
_FORCE_INLINE_ Plane Transform::xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const {
// Transform a single point on the plane.
Vector3 point = p_plane.normal * p_plane.d;
point = xform(point);
// Use inverse transpose for correct normals with non-uniform scaling.
Vector3 normal = p_basis_inverse_transpose.xform(p_plane.normal);
normal.normalize();
real_t d = normal.dot(point);
return Plane(normal, d);
}
_FORCE_INLINE_ Plane Transform::xform_inv_fast(const Plane &p_plane, const Transform &p_inverse, const Basis &p_basis_transpose) {
// Transform a single point on the plane.
Vector3 point = p_plane.normal * p_plane.d;
point = p_inverse.xform(point);
// Note that instead of precalculating the transpose, an alternative
// would be to use the transpose for the basis transform.
// However that would be less SIMD friendly (requiring a swizzle).
// So the cost is one extra precalced value in the calling code.
// This is probably worth it, as this could be used in bottleneck areas. And
// where it is not a bottleneck, the non-fast method is fine.
// Use transpose for correct normals with non-uniform scaling.
Vector3 normal = p_basis_transpose.xform(p_plane.normal);
normal.normalize();
real_t d = normal.dot(point);
return Plane(normal, d);
}
#endif // TRANSFORM_H

274
core/math/transform_2d.cpp Normal file
View File

@ -0,0 +1,274 @@
/*************************************************************************/
/* transform_2d.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#include "transform_2d.h"
/** Generic swap template */
#ifndef SWAP
#define SWAP(m_x, m_y) __swap_tmpl((m_x), (m_y))
template <class T>
inline void __swap_tmpl(T &x, T &y) {
T aux = x;
x = y;
y = aux;
}
#endif //swap
void Transform2D::invert() {
// FIXME: this function assumes the basis is a rotation matrix, with no scaling.
// Transform2D::affine_inverse can handle matrices with scaling, so GDScript should eventually use that.
SWAP(elements[0][1], elements[1][0]);
elements[2] = basis_xform(-elements[2]);
}
Transform2D Transform2D::inverse() const {
Transform2D inv = *this;
inv.invert();
return inv;
}
void Transform2D::affine_invert() {
real_t det = basis_determinant();
#ifdef MATH_CHECKS
ERR_FAIL_COND(det == 0);
#endif
real_t idet = 1.0 / det;
SWAP(elements[0][0], elements[1][1]);
elements[0] *= Vector2(idet, -idet);
elements[1] *= Vector2(-idet, idet);
elements[2] = basis_xform(-elements[2]);
}
Transform2D Transform2D::affine_inverse() const {
Transform2D inv = *this;
inv.affine_invert();
return inv;
}
void Transform2D::rotate(real_t p_phi) {
*this = Transform2D(p_phi, Vector2()) * (*this);
}
real_t Transform2D::get_rotation() const {
return Math::atan2(elements[0].y, elements[0].x);
}
void Transform2D::set_rotation(real_t p_rot) {
Vector2 scale = get_scale();
real_t cr = Math::cos(p_rot);
real_t sr = Math::sin(p_rot);
elements[0][0] = cr;
elements[0][1] = sr;
elements[1][0] = -sr;
elements[1][1] = cr;
set_scale(scale);
}
Transform2D::Transform2D(real_t p_rot, const Vector2 &p_pos) {
real_t cr = Math::cos(p_rot);
real_t sr = Math::sin(p_rot);
elements[0][0] = cr;
elements[0][1] = sr;
elements[1][0] = -sr;
elements[1][1] = cr;
elements[2] = p_pos;
}
Vector2 Transform2D::get_scale() const {
real_t det_sign = SGN(basis_determinant());
return Vector2(elements[0].length(), det_sign * elements[1].length());
}
void Transform2D::set_scale(const Vector2 &p_scale) {
elements[0].normalize();
elements[1].normalize();
elements[0] *= p_scale.x;
elements[1] *= p_scale.y;
}
void Transform2D::scale(const Vector2 &p_scale) {
scale_basis(p_scale);
elements[2] *= p_scale;
}
void Transform2D::scale_basis(const Vector2 &p_scale) {
elements[0][0] *= p_scale.x;
elements[0][1] *= p_scale.y;
elements[1][0] *= p_scale.x;
elements[1][1] *= p_scale.y;
}
void Transform2D::translate(real_t p_tx, real_t p_ty) {
translate(Vector2(p_tx, p_ty));
}
void Transform2D::translate(const Vector2 &p_translation) {
elements[2] += basis_xform(p_translation);
}
void Transform2D::orthonormalize() {
// Gram-Schmidt Process
Vector2 x = elements[0];
Vector2 y = elements[1];
x.normalize();
y = (y - x * (x.dot(y)));
y.normalize();
elements[0] = x;
elements[1] = y;
}
Transform2D Transform2D::orthonormalized() const {
Transform2D on = *this;
on.orthonormalize();
return on;
}
bool Transform2D::is_equal_approx(const Transform2D &p_transform) const {
return elements[0].is_equal_approx(p_transform.elements[0]) && elements[1].is_equal_approx(p_transform.elements[1]) && elements[2].is_equal_approx(p_transform.elements[2]);
}
bool Transform2D::operator==(const Transform2D &p_transform) const {
for (int i = 0; i < 3; i++) {
if (elements[i] != p_transform.elements[i]) {
return false;
}
}
return true;
}
bool Transform2D::operator!=(const Transform2D &p_transform) const {
for (int i = 0; i < 3; i++) {
if (elements[i] != p_transform.elements[i]) {
return true;
}
}
return false;
}
void Transform2D::operator*=(const Transform2D &p_transform) {
elements[2] = xform(p_transform.elements[2]);
real_t x0, x1, y0, y1;
x0 = tdotx(p_transform.elements[0]);
x1 = tdoty(p_transform.elements[0]);
y0 = tdotx(p_transform.elements[1]);
y1 = tdoty(p_transform.elements[1]);
elements[0][0] = x0;
elements[0][1] = x1;
elements[1][0] = y0;
elements[1][1] = y1;
}
Transform2D Transform2D::operator*(const Transform2D &p_transform) const {
Transform2D t = *this;
t *= p_transform;
return t;
}
Transform2D Transform2D::scaled(const Vector2 &p_scale) const {
Transform2D copy = *this;
copy.scale(p_scale);
return copy;
}
Transform2D Transform2D::basis_scaled(const Vector2 &p_scale) const {
Transform2D copy = *this;
copy.scale_basis(p_scale);
return copy;
}
Transform2D Transform2D::untranslated() const {
Transform2D copy = *this;
copy.elements[2] = Vector2();
return copy;
}
Transform2D Transform2D::translated(const Vector2 &p_offset) const {
Transform2D copy = *this;
copy.translate(p_offset);
return copy;
}
Transform2D Transform2D::rotated(real_t p_phi) const {
Transform2D copy = *this;
copy.rotate(p_phi);
return copy;
}
real_t Transform2D::basis_determinant() const {
return elements[0].x * elements[1].y - elements[0].y * elements[1].x;
}
Transform2D Transform2D::interpolate_with(const Transform2D &p_transform, real_t p_c) const {
//extract parameters
Vector2 p1 = get_origin();
Vector2 p2 = p_transform.get_origin();
real_t r1 = get_rotation();
real_t r2 = p_transform.get_rotation();
Vector2 s1 = get_scale();
Vector2 s2 = p_transform.get_scale();
//slerp rotation
Vector2 v1(Math::cos(r1), Math::sin(r1));
Vector2 v2(Math::cos(r2), Math::sin(r2));
real_t dot = v1.dot(v2);
dot = CLAMP(dot, -1.0, 1.0);
Vector2 v;
if (dot > 0.9995) {
v = v1.lerp(v2, p_c).normalized(); //linearly interpolate to avoid numerical precision issues
} else {
real_t angle = p_c * Math::acos(dot);
Vector2 v3 = (v2 - v1 * dot).normalized();
v = v1 * Math::cos(angle) + v3 * Math::sin(angle);
}
//construct matrix
Transform2D res(Math::atan2(v.y, v.x), p1.lerp(p2, p_c));
res.scale_basis(s1.lerp(s2, p_c));
return res;
}
Transform2D::operator String() const {
return String(String() + elements[0] + ", " + elements[1] + ", " + elements[2]);
}

224
core/math/transform_2d.h Normal file
View File

@ -0,0 +1,224 @@
/*************************************************************************/
/* transform_2d.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef TRANSFORM_2D_H
#define TRANSFORM_2D_H
#include "core/math/rect2.h" // also includes vector2, math_funcs, and ustring
#include "core/containers/vector.h"
#include "core/typedefs.h"
#include "core/math/vector2.h"
#include "core/math/math_defs.h"
#include "core/math/math.h"
struct Transform2D {
// Warning #1: basis of Transform2D is stored differently from Basis. In terms of elements array, the basis matrix looks like "on paper":
// M = (elements[0][0] elements[1][0])
// (elements[0][1] elements[1][1])
// This is such that the columns, which can be interpreted as basis vectors of the coordinate system "painted" on the object, can be accessed as elements[i].
// Note that this is the opposite of the indices in mathematical texts, meaning: $M_{12}$ in a math book corresponds to elements[1][0] here.
// This requires additional care when working with explicit indices.
// See https://en.wikipedia.org/wiki/Row-_and_column-major_order for further reading.
// Warning #2: 2D be aware that unlike 3D code, 2D code uses a left-handed coordinate system: Y-axis points down,
// and angle is measure from +X to +Y in a clockwise-fashion.
Vector2 elements[3];
_FORCE_INLINE_ real_t tdotx(const Vector2 &v) const { return elements[0][0] * v.x + elements[1][0] * v.y; }
_FORCE_INLINE_ real_t tdoty(const Vector2 &v) const { return elements[0][1] * v.x + elements[1][1] * v.y; }
const Vector2 &operator[](int p_idx) const { return elements[p_idx]; }
Vector2 &operator[](int p_idx) { return elements[p_idx]; }
_FORCE_INLINE_ Vector2 get_axis(int p_axis) const {
ERR_FAIL_INDEX_V(p_axis, 3, Vector2());
return elements[p_axis];
}
_FORCE_INLINE_ void set_axis(int p_axis, const Vector2 &p_vec) {
ERR_FAIL_INDEX(p_axis, 3);
elements[p_axis] = p_vec;
}
void invert();
Transform2D inverse() const;
void affine_invert();
Transform2D affine_inverse() const;
void set_rotation(real_t p_rot);
real_t get_rotation() const;
_FORCE_INLINE_ void set_rotation_and_scale(real_t p_rot, const Vector2 &p_scale);
void rotate(real_t p_phi);
void scale(const Vector2 &p_scale);
void scale_basis(const Vector2 &p_scale);
void translate(real_t p_tx, real_t p_ty);
void translate(const Vector2 &p_translation);
real_t basis_determinant() const;
Vector2 get_scale() const;
void set_scale(const Vector2 &p_scale);
_FORCE_INLINE_ const Vector2 &get_origin() const { return elements[2]; }
_FORCE_INLINE_ void set_origin(const Vector2 &p_origin) { elements[2] = p_origin; }
Transform2D scaled(const Vector2 &p_scale) const;
Transform2D basis_scaled(const Vector2 &p_scale) const;
Transform2D translated(const Vector2 &p_offset) const;
Transform2D rotated(real_t p_phi) const;
Transform2D untranslated() const;
void orthonormalize();
Transform2D orthonormalized() const;
bool is_equal_approx(const Transform2D &p_transform) const;
bool operator==(const Transform2D &p_transform) const;
bool operator!=(const Transform2D &p_transform) const;
void operator*=(const Transform2D &p_transform);
Transform2D operator*(const Transform2D &p_transform) const;
Transform2D interpolate_with(const Transform2D &p_transform, real_t p_c) const;
_FORCE_INLINE_ Vector2 basis_xform(const Vector2 &p_vec) const;
_FORCE_INLINE_ Vector2 basis_xform_inv(const Vector2 &p_vec) const;
_FORCE_INLINE_ Vector2 xform(const Vector2 &p_vec) const;
_FORCE_INLINE_ Vector2 xform_inv(const Vector2 &p_vec) const;
_FORCE_INLINE_ Rect2 xform(const Rect2 &p_rect) const;
_FORCE_INLINE_ Rect2 xform_inv(const Rect2 &p_rect) const;
_FORCE_INLINE_ Vector<Vector2> xform(const Vector<Vector2> &p_array) const;
_FORCE_INLINE_ Vector<Vector2> xform_inv(const Vector<Vector2> &p_array) const;
operator String() const;
Transform2D(real_t xx, real_t xy, real_t yx, real_t yy, real_t ox, real_t oy) {
elements[0][0] = xx;
elements[0][1] = xy;
elements[1][0] = yx;
elements[1][1] = yy;
elements[2][0] = ox;
elements[2][1] = oy;
}
Transform2D(real_t p_rot, const Vector2 &p_pos);
Transform2D() {
elements[0][0] = 1.0;
elements[1][1] = 1.0;
}
};
Vector2 Transform2D::basis_xform(const Vector2 &p_vec) const {
return Vector2(
tdotx(p_vec),
tdoty(p_vec));
}
Vector2 Transform2D::basis_xform_inv(const Vector2 &p_vec) const {
return Vector2(
elements[0].dot(p_vec),
elements[1].dot(p_vec));
}
Vector2 Transform2D::xform(const Vector2 &p_vec) const {
return Vector2(
tdotx(p_vec),
tdoty(p_vec)) +
elements[2];
}
Vector2 Transform2D::xform_inv(const Vector2 &p_vec) const {
Vector2 v = p_vec - elements[2];
return Vector2(
elements[0].dot(v),
elements[1].dot(v));
}
Rect2 Transform2D::xform(const Rect2 &p_rect) const {
Vector2 x = elements[0] * p_rect.w;
Vector2 y = elements[1] * p_rect.h;
Vector2 pos = xform(Vector2(p_rect.x, p_rect.y));
Rect2 new_rect;
new_rect.x = pos.x;
new_rect.y = pos.y;
new_rect.expand_to(pos + x);
new_rect.expand_to(pos + y);
new_rect.expand_to(pos + x + y);
return new_rect;
}
void Transform2D::set_rotation_and_scale(real_t p_rot, const Vector2 &p_scale) {
elements[0][0] = Math::cos(p_rot) * p_scale.x;
elements[1][1] = Math::cos(p_rot) * p_scale.y;
elements[1][0] = -Math::sin(p_rot) * p_scale.y;
elements[0][1] = Math::sin(p_rot) * p_scale.x;
}
Rect2 Transform2D::xform_inv(const Rect2 &p_rect) const {
Vector2 ends[4] = {
xform_inv(Vector2(p_rect.x, p_rect.y)),
xform_inv(Vector2(p_rect.x, p_rect.y + p_rect.h)),
xform_inv(Vector2(p_rect.x + p_rect.w, p_rect.y + p_rect.h)),
xform_inv(Vector2(p_rect.x + p_rect.w, p_rect.y))
};
Rect2 new_rect;
new_rect.x = ends[0].x;
new_rect.y = ends[0].y;
new_rect.expand_to(ends[1]);
new_rect.expand_to(ends[2]);
new_rect.expand_to(ends[3]);
return new_rect;
}
Vector<Vector2> Transform2D::xform(const Vector<Vector2> &p_array) const {
Vector<Vector2> array;
array.resize(p_array.size());
for (int i = 0; i < p_array.size(); ++i) {
array[i] = xform(p_array[i]);
}
return array;
}
Vector<Vector2> Transform2D::xform_inv(const Vector<Vector2> &p_array) const {
Vector<Vector2> array;
array.resize(p_array.size());
for (int i = 0; i < p_array.size(); ++i) {
array[i] = xform_inv(p_array[i]);
}
return array;
}
#endif // TRANSFORM_2D_H