Skip to content

Commit

Permalink
Add Rotation2d (#11658)
Browse files Browse the repository at this point in the history
# Objective

Rotating vectors is a very common task. It is required for a variety of
things both within Bevy itself and in many third party plugins, for
example all over physics and collision detection, and for things like
Bevy's bounding volumes and several gizmo implementations.

For 3D, we can do this using a `Quat`, but for 2D, we do not have a
clear and efficient option. `Mat2` can be used for rotating vectors if
created using `Mat2::from_angle`, but this is not obvious to many users,
it doesn't have many rotation helpers, and the type does not give any
guarantees that it represents a valid rotation.

We should have a proper type for 2D rotations. In addition to allowing
for potential optimization, it would allow us to have a consistent and
explicitly documented representation used throughout the engine, i.e.
counterclockwise and in radians.

## Representation

The mathematical formula for rotating a 2D vector is the following:

```
new_x = x * cos - y * sin
new_y = x * sin + y * cos
```

Here, `sin` and `cos` are the sine and cosine of the rotation angle.
Computing these every time when a vector needs to be rotated can be
expensive, so the rotation shouldn't be just an `f32` angle. Instead, it
is often more efficient to represent the rotation using the sine and
cosine of the angle instead of storing the angle itself. This can be
freely passed around and reused without unnecessary computations.

The two options are either a 2x2 rotation matrix or a unit complex
number where the cosine is the real part and the sine is the imaginary
part. These are equivalent for the most part, but the unit complex
representation is a bit more memory efficient (two `f32`s instead of
four), so I chose that. This is like Nalgebra's
[`UnitComplex`](https://docs.rs/nalgebra/latest/nalgebra/geometry/type.UnitComplex.html)
type, which can be used for the
[`Rotation2`](https://docs.rs/nalgebra/latest/nalgebra/geometry/type.Rotation2.html)
type.

## Implementation

Add a `Rotation2d` type represented as a unit complex number:

```rust
/// A counterclockwise 2D rotation in radians.
///
/// The rotation angle is wrapped to be within the `]-pi, pi]` range.
pub struct Rotation2d {
    /// The cosine of the rotation angle in radians.
    ///
    /// This is the real part of the unit complex number representing the rotation.
    pub cos: f32,
    /// The sine of the rotation angle in radians.
    ///
    /// This is the imaginary part of the unit complex number representing the rotation.
    pub sin: f32,
}
```

Using it is similar to using `Quat`, but in 2D:

```rust
let rotation = Rotation2d::radians(PI / 2.0);

// Rotate vector (also works on Direction2d!)
assert_eq!(rotation * Vec2::X, Vec2::Y);

// Get angle as degrees
assert_eq!(rotation.as_degrees(), 90.0);

// Getting sin and cos is free
let (sin, cos) = rotation.sin_cos();

// "Subtract" rotations
let rotation2 = Rotation2d::FRAC_PI_4; // there are constants!
let diff = rotation * rotation2.inverse();
assert_eq!(diff.as_radians(), PI / 4.0);

// This is equivalent to the above
assert_eq!(rotation2.angle_between(rotation), PI / 4.0);

// Lerp
let rotation1 = Rotation2d::IDENTITY;
let rotation2 = Rotation2d::FRAC_PI_2;
let result = rotation1.lerp(rotation2, 0.5);
assert_eq!(result.as_radians(), std::f32::consts::FRAC_PI_4);

// Slerp
let rotation1 = Rotation2d::FRAC_PI_4);
let rotation2 = Rotation2d::degrees(-180.0); // we can use degrees too!
let result = rotation1.slerp(rotation2, 1.0 / 3.0);
assert_eq!(result.as_radians(), std::f32::consts::FRAC_PI_2);
```

There's also a `From<f32>` implementation for `Rotation2d`, which means
that methods can still accept radians as floats if the argument uses
`impl Into<Rotation2d>`. This means that adding `Rotation2d` shouldn't
even be a breaking change.

---

## Changelog

- Added `Rotation2d`
- Bounding volume methods now take an `impl Into<Rotation2d>`
- Gizmo methods with rotation now take an `impl Into<Rotation2d>`

## Future use cases

- Collision detection (a type like this is quite essential considering
how common vector rotations are)
- `Transform` helpers (e.g. return a 2D rotation about the Z axis from a
`Transform`)
- The rotation used for `Transform2d` (#8268)
- More gizmos, maybe meshes... everything in 2D that uses rotation

---------

Co-authored-by: Tristan Guichaoua <[email protected]>
Co-authored-by: Robert Walter <[email protected]>
Co-authored-by: IQuick 143 <[email protected]>
  • Loading branch information
4 people authored Mar 11, 2024
1 parent 9cd3165 commit f89af05
Show file tree
Hide file tree
Showing 12 changed files with 797 additions and 80 deletions.
12 changes: 9 additions & 3 deletions crates/bevy_gizmos/src/gizmos.rs
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ use bevy_ecs::{
system::{Deferred, ReadOnlySystemParam, Res, Resource, SystemBuffer, SystemMeta, SystemParam},
world::{unsafe_world_cell::UnsafeWorldCell, World},
};
use bevy_math::{Dir3, Mat2, Quat, Vec2, Vec3};
use bevy_math::{Dir3, Quat, Rotation2d, Vec2, Vec3};
use bevy_transform::TransformPoint;

use crate::{
Expand Down Expand Up @@ -590,11 +590,17 @@ impl<'w, 's, T: GizmoConfigGroup> Gizmos<'w, 's, T> {
/// # bevy_ecs::system::assert_is_system(system);
/// ```
#[inline]
pub fn rect_2d(&mut self, position: Vec2, rotation: f32, size: Vec2, color: impl Into<Color>) {
pub fn rect_2d(
&mut self,
position: Vec2,
rotation: impl Into<Rotation2d>,
size: Vec2,
color: impl Into<Color>,
) {
if !self.enabled {
return;
}
let rotation = Mat2::from_angle(rotation);
let rotation: Rotation2d = rotation.into();
let [tl, tr, br, bl] = rect_inner(size).map(|vec2| position + rotation * vec2);
self.linestrip_2d([tl, tr, br, bl, tl], color);
}
Expand Down
3 changes: 2 additions & 1 deletion crates/bevy_math/Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ keywords = ["bevy"]
glam = { version = "0.25", features = ["bytemuck"] }
thiserror = "1.0"
serde = { version = "1", features = ["derive"], optional = true }
libm = { version = "0.2", optional = true }
approx = { version = "0.5", optional = true }

[dev-dependencies]
Expand All @@ -25,7 +26,7 @@ approx = ["dep:approx", "glam/approx"]
mint = ["glam/mint"]
# Enable libm mathematical functions for glam types to ensure consistent outputs
# across platforms at the cost of losing hardware-level optimization using intrinsics
libm = ["glam/libm"]
libm = ["dep:libm", "glam/libm"]
# Enable assertions to check the validity of parameters passed to glam
glam_assert = ["glam/glam-assert"]
# Enable assertions in debug builds to check the validity of parameters passed to glam
Expand Down
67 changes: 42 additions & 25 deletions crates/bevy_math/src/bounding/bounded2d/mod.rs
Original file line number Diff line number Diff line change
@@ -1,9 +1,7 @@
mod primitive_impls;

use glam::Mat2;

use super::{BoundingVolume, IntersectsVolume};
use crate::prelude::Vec2;
use crate::prelude::{Mat2, Rotation2d, Vec2};

/// Computes the geometric center of the given set of points.
#[inline(always)]
Expand All @@ -21,10 +19,11 @@ fn point_cloud_2d_center(points: &[Vec2]) -> Vec2 {
pub trait Bounded2d {
/// Get an axis-aligned bounding box for the shape with the given translation and rotation.
/// The rotation is in radians, counterclockwise, with 0 meaning no rotation.
fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d;
fn aabb_2d(&self, translation: Vec2, rotation: impl Into<Rotation2d>) -> Aabb2d;
/// Get a bounding circle for the shape
/// The rotation is in radians, counterclockwise, with 0 meaning no rotation.
fn bounding_circle(&self, translation: Vec2, rotation: f32) -> BoundingCircle;
fn bounding_circle(&self, translation: Vec2, rotation: impl Into<Rotation2d>)
-> BoundingCircle;
}

/// A 2D axis-aligned bounding box, or bounding rectangle
Expand Down Expand Up @@ -55,10 +54,14 @@ impl Aabb2d {
///
/// Panics if the given set of points is empty.
#[inline(always)]
pub fn from_point_cloud(translation: Vec2, rotation: f32, points: &[Vec2]) -> Aabb2d {
pub fn from_point_cloud(
translation: Vec2,
rotation: impl Into<Rotation2d>,
points: &[Vec2],
) -> Aabb2d {
// Transform all points by rotation
let rotation_mat = Mat2::from_angle(rotation);
let mut iter = points.iter().map(|point| rotation_mat * *point);
let rotation: Rotation2d = rotation.into();
let mut iter = points.iter().map(|point| rotation * *point);

let first = iter
.next()
Expand Down Expand Up @@ -94,7 +97,7 @@ impl Aabb2d {

impl BoundingVolume for Aabb2d {
type Translation = Vec2;
type Rotation = f32;
type Rotation = Rotation2d;
type HalfSize = Vec2;

#[inline(always)]
Expand Down Expand Up @@ -157,7 +160,11 @@ impl BoundingVolume for Aabb2d {
/// can cause the AABB to grow indefinitely. Avoid applying multiple rotations to the same AABB,
/// and consider storing the original AABB and rotating that every time instead.
#[inline(always)]
fn transformed_by(mut self, translation: Self::Translation, rotation: Self::Rotation) -> Self {
fn transformed_by(
mut self,
translation: Self::Translation,
rotation: impl Into<Self::Rotation>,
) -> Self {
self.transform_by(translation, rotation);
self
}
Expand All @@ -170,7 +177,11 @@ impl BoundingVolume for Aabb2d {
/// can cause the AABB to grow indefinitely. Avoid applying multiple rotations to the same AABB,
/// and consider storing the original AABB and rotating that every time instead.
#[inline(always)]
fn transform_by(&mut self, translation: Self::Translation, rotation: Self::Rotation) {
fn transform_by(
&mut self,
translation: Self::Translation,
rotation: impl Into<Self::Rotation>,
) {
self.rotate_by(rotation);
self.translate_by(translation);
}
Expand All @@ -189,7 +200,7 @@ impl BoundingVolume for Aabb2d {
/// can cause the AABB to grow indefinitely. Avoid applying multiple rotations to the same AABB,
/// and consider storing the original AABB and rotating that every time instead.
#[inline(always)]
fn rotated_by(mut self, rotation: Self::Rotation) -> Self {
fn rotated_by(mut self, rotation: impl Into<Self::Rotation>) -> Self {
self.rotate_by(rotation);
self
}
Expand All @@ -202,11 +213,14 @@ impl BoundingVolume for Aabb2d {
/// can cause the AABB to grow indefinitely. Avoid applying multiple rotations to the same AABB,
/// and consider storing the original AABB and rotating that every time instead.
#[inline(always)]
fn rotate_by(&mut self, rotation: Self::Rotation) {
let rot_mat = Mat2::from_angle(rotation);
let abs_rot_mat = Mat2::from_cols(rot_mat.x_axis.abs(), rot_mat.y_axis.abs());
fn rotate_by(&mut self, rotation: impl Into<Self::Rotation>) {
let rotation: Rotation2d = rotation.into();
let abs_rot_mat = Mat2::from_cols(
Vec2::new(rotation.cos, rotation.sin),
Vec2::new(rotation.sin, rotation.cos),
);
let half_size = abs_rot_mat * self.half_size();
*self = Self::new(rot_mat * self.center(), half_size);
*self = Self::new(rotation * self.center(), half_size);
}
}

Expand Down Expand Up @@ -431,7 +445,12 @@ impl BoundingCircle {
///
/// The bounding circle is not guaranteed to be the smallest possible.
#[inline(always)]
pub fn from_point_cloud(translation: Vec2, rotation: f32, points: &[Vec2]) -> BoundingCircle {
pub fn from_point_cloud(
translation: Vec2,
rotation: impl Into<Rotation2d>,
points: &[Vec2],
) -> BoundingCircle {
let rotation: Rotation2d = rotation.into();
let center = point_cloud_2d_center(points);
let mut radius_squared = 0.0;

Expand All @@ -443,10 +462,7 @@ impl BoundingCircle {
}
}

BoundingCircle::new(
Mat2::from_angle(rotation) * center + translation,
radius_squared.sqrt(),
)
BoundingCircle::new(rotation * center + translation, radius_squared.sqrt())
}

/// Get the radius of the bounding circle
Expand Down Expand Up @@ -476,7 +492,7 @@ impl BoundingCircle {

impl BoundingVolume for BoundingCircle {
type Translation = Vec2;
type Rotation = f32;
type Rotation = Rotation2d;
type HalfSize = f32;

#[inline(always)]
Expand Down Expand Up @@ -531,13 +547,14 @@ impl BoundingVolume for BoundingCircle {
}

#[inline(always)]
fn translate_by(&mut self, translation: Vec2) {
fn translate_by(&mut self, translation: Self::Translation) {
self.center += translation;
}

#[inline(always)]
fn rotate_by(&mut self, rotation: f32) {
self.center = Mat2::from_angle(rotation) * self.center;
fn rotate_by(&mut self, rotation: impl Into<Self::Rotation>) {
let rotation: Rotation2d = rotation.into();
self.center = rotation * self.center;
}
}

Expand Down
Loading

0 comments on commit f89af05

Please sign in to comment.