Skip to content

Commit

Permalink
add: circle bbox
Browse files Browse the repository at this point in the history
  • Loading branch information
@tk-yoshimura
tk-yoshimura committed Dec 5, 2024
1 parent 13158f8 commit d85c39a
Show file tree
Hide file tree
Showing 2 changed files with 187 additions and 0 deletions.
128 changes: 128 additions & 0 deletions DoubleDoubleGeometry/Geometry3D/Circle3D.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,6 +165,104 @@ public void Deconstruct(out Vector3D center, out ddouble radius, out Quaternion
[DebuggerBrowsable(DebuggerBrowsableState.Never)]
public static Circle3D Zero { get; } = new(Vector3D.Zero, ddouble.Zero, Quaternion.Zero, 0);

[DebuggerBrowsable(DebuggerBrowsableState.Never)]
private BoundingBox3D bbox = null;
[DebuggerBrowsable(DebuggerBrowsableState.Never)]
public BoundingBox3D BoundingBox {
get {
if (bbox is not null) {
return bbox;
}

CircleImplicitParameter param = new(Radius, Rotation);

(ddouble X, ddouble Y, ddouble Z) = Plane.Normal;

(ddouble A, ddouble B, ddouble C, ddouble D, ddouble E, ddouble F, ddouble G)
= (param.A, param.B, param.C, param.D, param.E, param.F, param.G);

int max_index = Vector3D.MaxAbsIndex(Plane.Normal);

ddouble x2 = X * X, y2 = Y * Y, z2 = Z * Z;

ddouble x = ddouble.NaN, y = ddouble.NaN, z = ddouble.NaN, emax = 0d;

{
ddouble a = B * x2 + A * y2 - D * Y * X;
ddouble b = F * x2 - (D * Z + E * Y) * X + 2d * A * Y * Z;
ddouble c = C * x2 + A * z2 - E * Z * X;
ddouble f = G * x2;

ddouble e = b * b - 4d * a * c;
ddouble m = f / e;

ddouble yt = 2d * ddouble.Sqrt(c * m);
ddouble zt = 2d * ddouble.Sqrt(a * m);

if (ddouble.Abs(e) > emax) {
emax = ddouble.Abs(e);
y = yt;
z = zt;
}
}
{
ddouble a = C * y2 + B * z2 - F * Z * Y;
ddouble b = E * y2 - (D * Z + F * X) * Y + 2d * B * Z * X;
ddouble c = A * y2 + B * x2 - D * X * Y;
ddouble f = G * y2;

ddouble e = b * b - 4d * a * c;
ddouble m = f / e;

ddouble zt = 2d * ddouble.Sqrt(c * m);
ddouble xt = 2d * ddouble.Sqrt(a * m);

if (ddouble.Abs(e) > emax) {
emax = ddouble.Abs(e);
z = zt;
x = xt;
}
else if (ddouble.IsNaN(z) && ddouble.IsFinite(zt)) {
z = zt;
}
}
{
ddouble a = A * z2 + C * x2 - E * X * Z;
ddouble b = D * z2 - (F * X + E * Y) * Z + 2d * C * X * Y;
ddouble c = B * z2 + C * y2 - F * Y * Z;
ddouble f = G * z2;

ddouble e = b * b - 4d * a * c;
ddouble m = f / e;

ddouble xt = 2d * ddouble.Sqrt(c * m);
ddouble yt = 2d * ddouble.Sqrt(a * m);

if (ddouble.Abs(e) > emax) {
emax = ddouble.Abs(e);
x = xt;
y = yt;
}
else {
if (ddouble.IsNaN(x) && ddouble.IsFinite(xt)) {
x = xt;
}
if (ddouble.IsNaN(y) && ddouble.IsFinite(yt)) {
y = yt;
}
}
}

if (emax > 0d) {
x = ddouble.IsFinite(x) ? x : 0d;
y = ddouble.IsFinite(y) ? y : 0d;
z = ddouble.IsFinite(z) ? z : 0d;
}

return bbox ??= new BoundingBox3D(Center, (x, y, z));
}
}

public static bool IsNaN(Circle3D g) {
return Vector3D.IsNaN(g.Center) || ddouble.IsNaN(g.Radius) || Quaternion.IsNaN(g.Rotation);
}
Expand Down Expand Up @@ -231,5 +329,35 @@ public bool Equals(Circle3D other) {
public override int GetHashCode() {
return Center.GetHashCode() ^ Radius.GetHashCode() ^ Rotation.GetHashCode();
}

// A x^2 + B y^2 + C z^2 + D x y + E x z + F y z + G = 0
private class CircleImplicitParameter {
public readonly ddouble A, B, C, D, E, F, G;

public CircleImplicitParameter(ddouble radius, Quaternion rotation) {
ddouble r2 = radius * radius;

Matrix3D s = Matrix3D.Scale(1d, 1d, 0d);
Matrix3D r = new(rotation);
Matrix3D ri = new(rotation.Conj);

Matrix3D m = r * s * ri;

this.A = m.E00;
this.B = m.E11;
this.C = m.E22;
this.D = 2d * m.E01;
this.E = 2d * m.E02;
this.F = 2d * m.E12;
this.G = -r2;
}

public static implicit operator
(ddouble a, ddouble b, ddouble c,
ddouble d, ddouble e, ddouble f, ddouble g)(CircleImplicitParameter param) {

return (param.A, param.B, param.C, param.D, param.E, param.F, param.G);
}
}
}
}
59 changes: 59 additions & 0 deletions DoubleDoubleGeometryTest/Geometry3D/Circle3DTests.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,65 @@ public void PointTest() {
Vector3DAssert.AreEqual(q * (circle1.Point(ddouble.Pi / 2) * 5 + (2, 3, 4)), circle5.Point(ddouble.Pi / 2), 1e-29);
}

[TestMethod()]
public void BoundingBoxTest() {
Circle3D circle1 = new((0, 0, 0), 8, Quaternion.One);
Circle3D circle2 = new((0, 0, 0), 8, (0, 1, 0));
Circle3D circle3 = new((0, 0, 0), 8, (1, 0, 0));
Circle3D circle4 = new((0, 0, 0), 8, (3, -6, 4, 7));
Circle3D circle5 = new((0, 0, 0), 8, (1, -2, 3, 4));
Circle3D circle6 = new((1, 2, 3), 8, (1, -2, 3, 4));
Circle3D circle7 = new((0, 0, 0), 8, (3, 4, 1, -2));

Vector3DAssert.AreEqual((8, 8, 0), circle1.BoundingBox.Scale, 1e-30);
Vector3DAssert.AreEqual((8, 0, 8), circle2.BoundingBox.Scale, 1e-30);
Vector3DAssert.AreEqual((0, 8, 8), circle3.BoundingBox.Scale, 1e-30);

bool any_outside = false;
for (double t = 0; t < 8; t += 0.25) {
Assert.IsTrue(circle4.BoundingBox.Inside(circle4.Point(t) * 0.9999));

if (!circle4.BoundingBox.Inside(circle4.Point(t) * 1.01)) {
any_outside = true;
}
}

Assert.IsTrue(any_outside);

any_outside = false;
for (double t = 0; t < 8; t += 0.25) {
Assert.IsTrue(circle5.BoundingBox.Inside(circle5.Point(t) * 0.9999));

if (!circle5.BoundingBox.Inside(circle5.Point(t) * 1.01)) {
any_outside = true;
}
}

Assert.IsTrue(any_outside);

any_outside = false;
for (double t = 0; t < 8; t += 0.25) {
Assert.IsTrue(circle6.BoundingBox.Inside(circle5.Point(t) * 0.9999 + (1, 2, 3)));

if (!circle6.BoundingBox.Inside(circle6.Point(t) * 1.01)) {
any_outside = true;
}
}

Assert.IsTrue(any_outside);

any_outside = false;
for (double t = 0; t < 8; t += 0.25) {
Assert.IsTrue(circle7.BoundingBox.Inside(circle7.Point(t) * 0.9999));

if (!circle7.BoundingBox.Inside(circle7.Point(t) * 1.01)) {
any_outside = true;
}
}

Assert.IsTrue(any_outside);
}

[TestMethod()]
public void ValidTest() {
Assert.IsTrue(Circle3D.IsValid(new Circle3D((1, 3, 5), 2, (2, 4, 6))));
Expand Down

0 comments on commit d85c39a

Please sign in to comment.