public void TestPointRotation1()
{
KVector2 point = new KVector2(1, 0);
KTransform tx = new KTransform(KVector2.Zero, (float)(System.Math.PI / 2.0), new KVector2(1));
Assert.AreEqual(new KVector2(0, -1), tx.TransformPointWL(point));
}
public void TestWorldToLocal()
{
KVector2 point = new KVector2(31, -4);
KTransform tx = new KTransform(KVector2.One, (System.Math.PI / 2.0), new KVector2(5));
Assert.AreEqual(new KVector2(-1, -6), tx.TransformPointWL(point));
}
//FIXME: Bad CircleVsCircle test
private static bool CircleVsCircle(ref Contact contact)
{
Circle a = (Circle)contact.FixtureA.Collider;
Circle b = (Circle)contact.FixtureB.Collider;
//In the local worl A is a unit circle and B is an ellipse
KVector2 cB = a.Transform.TransformPointWL(b.Transform.TransformPointLW(b.LPosition));
KVector2 axisA = a.Transform.TransformDirWL(b.Transform.TransformDirLW(KVector2.XPos));
KVector2 axisB = a.Transform.TransformDirWL(b.Transform.TransformDirLW(KVector2.YPos));
KTransform kTransform = new KTransform(cB, a.Transform.Rotation + b.Transform.Rotation, KVector2.One);
KVector2 P = kTransform.TransformPointWL(a.LPosition);
double t = Math.PI / 4f;
KVector2 scale = new KVector2(axisA.Length(), axisB.Length());
KVector2 potCp = new KVector2();
for (int i = 0; i < 50; i++)
{
potCp = new KVector2(scale.X * Math.Cos(t), scale.Y * Math.Sin(t));
double ex = (scale.X * scale.X) - (scale.Y * scale.Y) * Math.Pow(Math.Cos(t), 3) / scale.X;
double ey = (scale.Y * scale.Y) - (scale.X * scale.X) * Math.Pow(Math.Sin(t), 3) / scale.Y;
KVector2 e = new KVector2(ex, ey);
KVector2 r = potCp - e;
KVector2 q = P - e;
double delta_c = r.Length() * Math.Asin((r.X * q.Y - r.Y * q.X) / (r.Length() * q.Length()));
double delta_t = delta_c / Math.Sqrt(scale.LengthSquared() - potCp.LengthSquared());
t += delta_t;
t += Math.Min(Math.PI / 2, Math.Max(0, t));
}
KVector2 wPos = a.Transform.TransformPointLW(kTransform.TransformPointLW(potCp));
KVector2 cDir = P - potCp;
KVector2 wDir = b.Transform.TransformDirLW(KVector2.Normalize(cDir) * (1 - cDir.Length()));
if (wDir.Length() >= 1)
{
return(false);
}
ContactPoint cp = new ContactPoint
{
WPosition = wPos,
WPenetration = wDir.Length()
};
contact.Manifold.WNormal = KVector2.Normalize(wDir);
contact.Manifold.ContactPoints[0] = cp;
contact.Manifold.Count = 1;
return(true);
}
