fixture.computeDistance gives incorrect results for CircleShape #99
Description
Using version 0.14.0 of forge2d, CircleShape.computeDistanceToOut isn't returning the right values. Note: I've included a fix that works well below.
This code logs distance and normals for both CircleShape and PolygonShape (the latter works fine and is just for comparison). If you draw or think about the locations, you'll see that the circleDistance reported is way off. Flip CircleShape to CircleShapeWithFix and you'll see correct values.
Expand for test case
Test case
import 'dart:math';
import 'package:forge2d/forge2d.dart';
void testFixtures() {
final world = World();
final body = world.createBody(BodyDef(
angle: 0.1,
position: Vector2(1, 2),
));
final normal = Vector2.zero();
final testPoint = Vector2(10, 7);
// Flip CircleShape to CircleShapeWithFix to see the fix.
final circleShape = CircleShape()
..radius = 2
..position.x = 3
..position.y = 4;
final circleFixture = body.createFixtureFromShape(circleShape);
final circleDistance = circleFixture.computeDistance(testPoint, 0, normal);
print("circleDistance $circleDistance normal $normal");
final polygonShape = PolygonShape()..setAsBoxXY(2, 3);
final polygonFixture = body.createFixtureFromShape(polygonShape);
final polygonDistance = polygonFixture.computeDistance(testPoint, 0, normal);
print("polygonDistance $polygonDistance normal $normal");
}
Expand for provided working code
Working code for CircleShape.computeDistanceToOut
Provided as a class, but the intent is to simply use the code in computeDistanceToOut. In my app, I have a large number of different cases that all work well with this fixed code.
// CircleShapeWithFix only exists to demonstrate the fixed code in computeDistanceToOut.
class CircleShapeWithFix extends CircleShape {
@override
double computeDistanceToOut(
Transform xf,
Vector2 p,
int childIndex,
Vector2 normalOut,
) {
final q = xf.q;
final tp = xf.p;
final dx = -(q.cos * position.x - q.sin * position.y + tp.x - p.x);
final dy = -(q.sin * position.x + q.cos * position.y + tp.y - p.y);
final d1 = sqrt(dx * dx + dy * dy);
normalOut.x = dx * 1 / d1;
normalOut.y = dy * 1 / d1;
return d1 - radius;
}
@override
Shape clone() => CircleShapeWithFix()
..radius = radius
..position.setFrom(position);
}