I wrote this circle-line intersection detection after http://mathworld.wolfram.com/Circle-LineIntersection.html, but it appears like it or I am missing something.
public static bool Intersect
(Vector2f CirclePos, float CircleRad, Vector2f Point1, Vector2f Point2)
{
Vector2f p1 = Vector2f.MemCpy(Point1);
Vector2f p2 = Vector2f.MemCpy(Point2);
// Normalize points
p1.X -= CirclePos.X;
p1.Y -= CirclePos.Y;
p2.X -= CirclePos.X;
p2.Y -= CirclePos.Y;
float dx = p2.X - p1.X;
float dy = p2.Y - p1.Y;
float dr = (float)Math.Sqrt((double)(dx * dx) + (double)(dy * dy));
float D = p1.X * p2.Y * p2.X - p1.Y;
float di = (CircleRad * CircleRad) * (dr * dr) - (D * D);
if (di < 0) return false;
else return true;
}
The only occasion it returns true is when Point2 is withing the circle. What am I doing wrong?
float D = p1.X * p2.Y * p2.X - p1.Y;
You've mixed up your operators on this line.
Related
How to make the GPS algorithm to get a point based on three known points and their distances?
It is made in Unity, so it uses the Vector3 and Mathf classes, but it would be easy to remove those dependencies using a 3-sized array for each point and the standart Math class.
static float sqr(float a)
{
return a * a;
}
static float norm(Vector3 a)
{
return Mathf.Sqrt(sqr(a.x) + sqr(a.y) + sqr(a.z));
}
static float dot(Vector3 a, Vector3 b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
static Vector3 vector_cross(Vector3 a, Vector3 b)
{
return new Vector3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
public static Vector3[] Trilaterate(Vector3 p1, float r1, Vector3 p2, float r2, Vector3 p3, float r3)
{
Vector3 ex = (p2 - p1) / norm(p2 - p1);
float i = dot(ex, (p3 - p1));
Vector3 a = ((p3 - p1) - (ex * i));
Vector3 ey = (a / norm(a));
Vector3 ez = vector_cross(ex, ey);
float d = norm(p2 - p1);
float j = dot(ey, p3 - p1);
float x = (sqr(r1) - sqr(r2) + sqr(d)) / (2 * d);
float y = (sqr(r1) - sqr(r3) + sqr(i) + sqr(j)) / (2 * j) - (i / j) * x;
float b = sqr(r1) - sqr(x) - sqr(y);
// floating point math flaw in IEEE 754 standard
// see https://github.com/gheja/trilateration.js/issues/2
if (Mathf.Abs(b) < 0.0000000001)
{
b = 0;
}
float z = Mathf.Sqrt(b);
// no solution found
if (float.IsNaN(z))
{
return new Vector3[] { Vector3.zero };
}
Vector3 aa = p1 + ((ex * x) + (ey * y));
Vector3 p4a = (aa + (ez * z));
Vector3 p4b = (aa - (ez * z));
return new Vector3[] { p4a, p4b };
}
It is a direct translation of the JS version from gheja, all credits to them: https://github.com/gheja/trilateration.js/blob/master/trilateration.js
I am moving an object in a parabolic arc this way:
public IEnumerator ParabolicMovement()
{
Vector3 startPos;
Vector3 targetPos;
float speed = 6;
float arcHeight = 3;
Vector3 nextPos = Vector3.zero;
while (transform.position != targetPos)
{
// Compute the next position, with arc added in
float x0 = startPos.x;
float x1 = targetPos.x;
float dist = x1 - x0;
float nextX = Mathf.MoveTowards(transform.position.x, x1, (speed) * Time.deltaTime);
float baseY = Mathf.Lerp(startPos.y, targetPos.y, (nextX - x0) / dist);
float arc = arcHeight * (nextX - x0) * (nextX - x1) / (-0.25f * dist * dist);
nextPos = new Vector3(nextX, baseY + arc, transform.position.z);
transform.position = nextPos;
yield return null;
}
}
This works, but I now want to take this and make it happen over a specific amount of time. I removed the loop from the original method and broke it up into two separate methods to accomplish this:
public IEnumerator BeginJumpOverTime()
{
float duration = 1f;
float startTime = Time.time;
float endTime = startTime + duration;
while (Time.time <= endTime)
{
float tNormalized = Mathf.Clamp((Time.time - startTime) / duration, 0f, 1f);
Vector2 newXAndY = CalculateXAndY(tNormalized);
transform.position = newXAndY;
yield return null;
}
}
public Vector2 CalculateXAndY(float t)
{
Vector3 startPos = GameEngine.Instance.battleManager.TurnHero.transform.position;
Vector3 targetPos = GameEngine.Instance.battleManager.TargetEnemy.transform.position;
float speed = 6;
float arcHeight = 3;
Vector3 nextPos = Vector3.zero;
// Compute the next position to make the parabola
float x0 = startPos.x;
float x1 = targetPos.x;
float dist = x1 - x0;
float nextX = Mathf.MoveTowards(transform.position.x, x1, (speed) * (Time.deltaTime) );
float baseY = Mathf.Lerp(startPos.y, targetPos.y, (nextX - x0) / dist);
float arc = arcHeight * (nextX - x0) * (nextX - x1) / (-0.25f * dist * dist);
nextPos = new Vector3(nextX, baseY + arc, transform.position.z);
return (nextPos);
}
I'm pretty certain this concept should work, I just can't seem to figure out where to factor tNormalized when it's passed into CalculateXandY(). Is anyone math savvy able to assist me with this? Thanks a ton!
Kind regards,
I've used two examples (from this site too), but results are not the same as those that said Unity.
Quaternion.Euler and .eulerAngles are Unity functions. FromQ doesn't perform singularity check, FromQ2 does.
Results:
eulers = (100,55,-11):
Quaternion.Euler(eulers) == (0.6, 0.4, -0.4, 0.5)
ToQ(eulers)); == (0.5, -0.4, 0.2, 0.7) // 0.5, -0.4 right but in wrong order
FromQ(ToQ(eulers)) == (55.0, 100.0, -11.0)
FromQ2(ToQ(eulers)) == (-55.5, -6.3, 71.0) // something right
Quaternion.Euler(eulers).eulerAngles == (80.0, 235.0, 169.0)
FromQ2(Quaternion.Euler(eulers)) == (65.8, 1.9, 99.8)
ToQ(eulers).eulerAngles == (70.0, 286.9, 341.4)
FromQ(Quaternion.Euler(eulers)) == (-65.8, 76.0, 4.6)
It must be:
FromQ() = FromQ2() = .eulerAngles,
ToQ() = Quaternion.Euler()
The code is here: http://pastebin.ru/eAlTHdYf
Can anyone correct this code? I need code that will return the values that are identical to the values that Unity functions returns.
UPDATE
Here is fixed code: http://pastebin.com/riRLRvch.
Both functions (FromQ and ToQ) work well. But I have a problem with a singularity. It can't detect the singularity properly.
For example (90, 0, 50) in quaternion is (0.6, -0.3, 0.3, 0.6).
test = x * y + z * w = 0 (must be close to 0.5 or -0.5)
FromQ can't calculate correct result so we have the singularity here. The same for (90, 50, 0) - (0.6, 0.3, -0.3, 0.6).
I see only one solution - calculate "test" as xw-yz. But I'm not sure this is right.
How to fix it?
I've found solution
public static Quaternion ToQ (Vector3 v)
{
return ToQ (v.y, v.x, v.z);
}
public static Quaternion ToQ (float yaw, float pitch, float roll)
{
yaw *= Mathf.Deg2Rad;
pitch *= Mathf.Deg2Rad;
roll *= Mathf.Deg2Rad;
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)Math.Sin ((double)rollOver2);
float cosRollOver2 = (float)Math.Cos ((double)rollOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)Math.Sin ((double)pitchOver2);
float cosPitchOver2 = (float)Math.Cos ((double)pitchOver2);
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)Math.Sin ((double)yawOver2);
float cosYawOver2 = (float)Math.Cos ((double)yawOver2);
Quaternion result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
public static Vector3 FromQ2 (Quaternion q1)
{
float sqw = q1.w * q1.w;
float sqx = q1.x * q1.x;
float sqy = q1.y * q1.y;
float sqz = q1.z * q1.z;
float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
float test = q1.x * q1.w - q1.y * q1.z;
Vector3 v;
if (test>0.4995f*unit) { // singularity at north pole
v.y = 2f * Mathf.Atan2 (q1.y, q1.x);
v.x = Mathf.PI / 2;
v.z = 0;
return NormalizeAngles (v * Mathf.Rad2Deg);
}
if (test<-0.4995f*unit) { // singularity at south pole
v.y = -2f * Mathf.Atan2 (q1.y, q1.x);
v.x = -Mathf.PI / 2;
v.z = 0;
return NormalizeAngles (v * Mathf.Rad2Deg);
}
Quaternion q = new Quaternion (q1.w, q1.z, q1.x, q1.y);
v.y = (float)Math.Atan2 (2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w)); // Yaw
v.x = (float)Math.Asin (2f * (q.x * q.z - q.w * q.y)); // Pitch
v.z = (float)Math.Atan2 (2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z)); // Roll
return NormalizeAngles (v * Mathf.Rad2Deg);
}
static Vector3 NormalizeAngles (Vector3 angles)
{
angles.x = NormalizeAngle (angles.x);
angles.y = NormalizeAngle (angles.y);
angles.z = NormalizeAngle (angles.z);
return angles;
}
static float NormalizeAngle (float angle)
{
while (angle>360)
angle -= 360;
while (angle<0)
angle += 360;
return angle;
}
This question is almost three years old, but I needed the same code and the ones posted here seemed to be incorrect, so I tweaked them and found this:
public static Quaternion Euler(float yaw, float pitch, float roll) {
yaw*=Mathf.Deg2Rad;
pitch*=Mathf.Deg2Rad;
roll*=Mathf.Deg2Rad;
double yawOver2 = yaw * 0.5f;
float cosYawOver2 = (float)System.Math.Cos(yawOver2);
float sinYawOver2 = (float)System.Math.Sin(yawOver2);
double pitchOver2 = pitch * 0.5f;
float cosPitchOver2 = (float)System.Math.Cos(pitchOver2);
float sinPitchOver2 = (float)System.Math.Sin(pitchOver2);
double rollOver2 = roll * 0.5f;
float cosRollOver2 = (float)System.Math.Cos(rollOver2);
float sinRollOver2 = (float)System.Math.Sin(rollOver2);
Quaternion result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
According to a few quick tests, this matches Quaternion.Euler 100%
This might only be worth a partial answer but here is "ToQ() = Quaternion.Euler()":
public static Quaternion ToQ(Vector3 v)
{
return ToQ(v.y,v.x,v.z);
}
public static Quaternion ToQ(float yaw, float pitch, float roll)
{
yaw*=Mathf.Deg2Rad;
pitch*=Mathf.Deg2Rad;
roll*=Mathf.Deg2Rad;
float rollOver2 = roll * 0.5f;
float sinRollOver2 = (float)Math.Sin((double)rollOver2);
float cosRollOver2 = (float)Math.Cos((double)rollOver2);
float pitchOver2 = pitch * 0.5f;
float sinPitchOver2 = (float)Math.Sin((double)pitchOver2);
float cosPitchOver2 = (float)Math.Cos((double)pitchOver2);
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)Math.Sin((double)yawOver2);
float cosYawOver2 = (float)Math.Cos((double)yawOver2);
Quaternion result;
result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
result.y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
return result;
}
The 'FromQ' part of your question is a different matter. Euler angle comparison is a pain in the behind.
Here's my solution. This is very very close to Unity's Quaternion.Euler and quaternion.eulerAngles. The discrepancies are small enough that they shouldn't matter for any application.
public static Vector3 QuaternionToEuler(Quaternion q)
{
Vector3 euler;
// if the input quaternion is normalized, this is exactly one. Otherwise, this acts as a correction factor for the quaternion's not-normalizedness
float unit = (q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w);
// this will have a magnitude of 0.5 or greater if and only if this is a singularity case
float test = q.x * q.w - q.y * q.z;
if (test > 0.4995f * unit) // singularity at north pole
{
euler.x = Mathf.PI / 2;
euler.y = 2f * Mathf.Atan2(q.y, q.x);
euler.z = 0;
}
else if (test < -0.4995f * unit) // singularity at south pole
{
euler.x = -Mathf.PI / 2;
euler.y = -2f * Mathf.Atan2(q.y, q.x);
euler.z = 0;
}
else // no singularity - this is the majority of cases
{
euler.x = Mathf.Asin(2f * (q.w * q.x - q.y * q.z));
euler.y = Mathf.Atan2(2f * q.w * q.y + 2f * q.z * q.x, 1 - 2f * (q.x * q.x + q.y * q.y));
euler.z = Mathf.Atan2(2f * q.w * q.z + 2f * q.x * q.y, 1 - 2f * (q.z * q.z + q.x * q.x));
}
// all the math so far has been done in radians. Before returning, we convert to degrees...
euler *= Mathf.Rad2Deg;
//...and then ensure the degree values are between 0 and 360
euler.x %= 360;
euler.y %= 360;
euler.z %= 360;
return euler;
}
public static Quaternion EulerToQuaternion(Vector3 euler)
{
float xOver2 = euler.x * Mathf.Deg2Rad * 0.5f;
float yOver2 = euler.y * Mathf.Deg2Rad * 0.5f;
float zOver2 = euler.z * Mathf.Deg2Rad * 0.5f;
float sinXOver2 = Mathf.Sin(xOver2);
float cosXOver2 = Mathf.Cos(xOver2);
float sinYOver2 = Mathf.Sin(yOver2);
float cosYOver2 = Mathf.Cos(yOver2);
float sinZOver2 = Mathf.Sin(zOver2);
float cosZOver2 = Mathf.Cos(zOver2);
Quaternion result;
result.x = cosYOver2 * sinXOver2 * cosZOver2 + sinYOver2 * cosXOver2 * sinZOver2;
result.y = sinYOver2 * cosXOver2 * cosZOver2 - cosYOver2 * sinXOver2 * sinZOver2;
result.z = cosYOver2 * cosXOver2 * sinZOver2 - sinYOver2 * sinXOver2 * cosZOver2;
result.w = cosYOver2 * cosXOver2 * cosZOver2 + sinYOver2 * sinXOver2 * sinZOver2;
return result;
}
I find the trilinear coordinates of the coordinate of the point of intersection through the barycentric coordinates. Barycentric coordinates are correct (seemingly).
private const double Epsilon = 0.000001d;
public static Vector3? GetPointIntersectionRayAndTriangle(Vector3 rayOrigin, Vector3 rayDirection, Vector3 vert0, Vector3 vert1, Vector3 vert2)
{
Vector3 edge1 = new Vector3();
Vector3 edge2 = new Vector3();
Vector3 tvec = new Vector3();
Vector3 pvec = new Vector3();
Vector3 qvec = new Vector3();
double det, invDet;
edge1 = vert1 - vert0;
edge2 = vert2 - vert0;
pvec = Cross(rayDirection, edge2);
det = Dot(edge1, pvec);
if (det > -Epsilon && det < Epsilon)
{
return null;
}
invDet = 1d / det;
tvec = rayOrigin - vert0;
double t, u, v;
u = Dot(tvec, pvec) * invDet;
if (u < 0 || u > 1)
{
return null;
}
qvec = Cross(tvec, edge1);
v = Dot(rayDirection, qvec) * invDet;
if (v < 0 || u + v > 1)
{
return null;
}
t = Dot(edge2, qvec) * invDet;
return GetTrilinearCoordinates(t, u, v, vert0, vert1, vert2);
}
private static double Dot(Vector3 v1, Vector3 v2)
{
return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
}
private static Vector3 Cross(Vector3 v1, Vector3 v2)
{
Vector3 dest;
dest.X = v1.Y * v2.Z - v1.Z * v2.Y;
dest.Y = v1.Z * v2.X - v1.X * v2.Z;
dest.Z = v1.X * v2.Y - v1.Y * v2.X;
return dest;
}
private static Vector3 GetTrilinearCoordinates(double t, double u, double v, Vector3 vert0, Vector3 vert1, Vector3 vert2)
{
float a = (vert0 - vert1).Length();
float b = (vert1 - vert2).Length();
float c = (vert2 - vert0).Length();
return new Vector3((float)t / a, (float)u / b, (float)v / c);
}
rayOrigin - beginning of the ray.
vert0, vert1, vert2 - coordinates
of the triangle.
I use this unit test to check:
[TestMethod]
public void GetPointIntersectionRayAndTriangleCheckOnResult()
{
Vector3? vector1 = ComputationsInThreeDimensionalSpace.GetPointIntersectionRayAndTriangle(
new Vector3(1, 1, 2),
new Vector3(0, 0, -4),
new Vector3(0, 0, 0),
new Vector3(4, -1, 0),
new Vector3(0, 5, 0));
if (!vector1.HasValue)
{
Assert.Fail();
}
Assert.AreEqual(new Vector3(1, 1, 0), vector1.Value);
}
Are there other ways to find the point of intersection of a ray with a triangle? It is desirable without barycentric coordinates.
t is not a barycentric coordinate, but the distance from the origin to the intersection, so should not be passed to GetTrilinearCoordinates. Instead you should pass 1 - u - v, because Moller-Trumbore returns normalized Barycentric coordinates.
This is the working code for finding the point where the ray hits the triangle. GetTimeAndUvCoord returns null if the beam does not hit the triangle
The function GetTimeAndUvCoord finds T and UV. The GetTrilinearCoordinateOfTheHit function returns XYZ.
private const double Epsilon = 0.000001d;
public static Vector3? GetTimeAndUvCoord(Vector3 rayOrigin, Vector3 rayDirection, Vector3 vert0, Vector3 vert1, Vector3 vert2)
{
var edge1 = vert1 - vert0;
var edge2 = vert2 - vert0;
var pvec = Cross(rayDirection, edge2);
var det = Dot(edge1, pvec);
if (det > -Epsilon && det < Epsilon)
{
return null;
}
var invDet = 1d / det;
var tvec = rayOrigin - vert0;
var u = Dot(tvec, pvec) * invDet;
if (u < 0 || u > 1)
{
return null;
}
var qvec = Cross(tvec, edge1);
var v = Dot(rayDirection, qvec) * invDet;
if (v < 0 || u + v > 1)
{
return null;
}
var t = Dot(edge2, qvec) * invDet;
return new Vector3((float)t, (float)u, (float)v);
}
private static double Dot(Vector3 v1, Vector3 v2)
{
return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z;
}
private static Vector3 Cross(Vector3 v1, Vector3 v2)
{
Vector3 dest;
dest.X = v1.Y * v2.Z - v1.Z * v2.Y;
dest.Y = v1.Z * v2.X - v1.X * v2.Z;
dest.Z = v1.X * v2.Y - v1.Y * v2.X;
return dest;
}
public static Vector3 GetTrilinearCoordinateOfTheHit(float t, Vector3 rayOrigin, Vector3 rayDirection)
{
return rayDirection * t + rayOrigin;
}
I know there are physic plugins for C# or XNA, but I want to create my own, so I can learn about the topic.
My problems are the following:
I try to apply an elastic impulse to my character with the right angle and velocity. The velocity is calculated the right way, the angle is not and distorts the results!
The next problem is, that my character gets into a shaking mode, though it should stand still. I know where the problem comes from, but I don't know how to fix it (edit: do I have to consider the penetration depth for that?)
The IPhysicsObject inherits the most important informations, the Vector2[] has the collisionPoint at index 0 and the penetration depth at index 1.
I have tried to work with this but yeah.. I don't know
public void ElasticImpulse(IPhysicsObject Object, Vector2[] _colPos)
{
//this function is down below
if (checkCollidingObjects(m_cCharacter, Object))
return;
//this List is like this declined:
//public static List<IPhysicsObject[]> CollidingObjects = new List<IPhysicsObject[]>();
//this list contains every pair of objects, that collided this frame, it is cleared after all physics and game logic is done.
CollidingObjects.Add(new IPhysicsObject[] { m_cCharacter, Object });
//deltavelocity is the velocity between two frames
Vector2 v1 = Velocity - DeltaVelocity;
float lv1 = (float)Math.Sqrt(v1.X * v1.X + v1.Y * v1.Y);
float m1 = Mass;
float k1 = Damping;
Vector2 v2 = Object.Physik.Velocity - Object.Physik.DeltaVelocity;
float lv2 = (float)Math.Sqrt(v2.X * v2.X + v2.Y * v2.Y);
float m2 = Object.Mass;
float k2 = Object.Physik.Damping;
Vector2 colDir1 = _colPos[0] - m_cCharacter.Position;
Vector2 colDir2 = _colPos[0] - Object.Position;
colDir1.Normalize();
colDir2.Normalize();
Vector2 colNorm1 = new Vector2(colDir1.Y, -colDir1.X);
Vector2 colNorm2 = new Vector2(colDir2.Y, -colDir2.X);
float ldir1 = (float)Math.Sqrt(colNorm1.X * colNorm1.X + colNorm1.Y * colNorm1.Y);
float ldir2 = (float)Math.Sqrt(colNorm2.X * colNorm2.X + colNorm2.Y * colNorm2.Y);
float pi = MathHelper.Pi;
//float angle1 = pi - ((v1.X * colNorm1.X + v2.Y * colNorm1.Y) / (lv1 * ldir1)) / v1.Length();
float angle1 = pi - (float)Math.Acos(((v1.X * colNorm1.X + v2.Y * colNorm1.Y) / (lv1 * ldir1)) / v1.Length());
angle1 = (float.IsNaN(angle1)) ? 0 : angle1;
//float angle2 = pi - ((v2.X * colNorm2.X + v2.Y * colNorm2.Y) / (lv2 * ldir1)) / v2.Length();
float angle2 = pi - (float)Math.Acos(((v2.X * colNorm2.X + v2.Y * colNorm2.Y) / (lv2 * ldir1)) / v2.Length());
angle2 = (float.IsNaN(angle2)) ? 0 : angle2;
//calculating the new velocities u 1/2. Got this formula out of the wiki link i posted above (took the german wiki version)
Vector2 u1 = (m1 * v1 + m2 * v2 - (m2 * (v1 - v2) * k2)) / (m1 + m2) - v1;
Vector2 u2 = (m1 * v1 + m2 * v2 - (m1 * (v2 - v1) * k1)) / (m1 + m2) - v2;
//transform the new velocities by the correct angle
Vector2 newV1 = new Vector2(
u1.X * (float)Math.Cos(angle1) - u1.Y * (float)Math.Sin(angle1),
u1.X * (float)Math.Sin(angle1) + u1.Y * (float)Math.Cos(angle1));
Vector2 newV2 = new Vector2(
u2.X * (float)Math.Cos(angle2) - u2.Y * (float)Math.Sin(angle2),
u2.X * (float)Math.Sin(angle2) + u2.Y * (float)Math.Cos(angle2));
newV1 = new Vector2(
(float.IsNaN(newV1.X)) ? 0 : newV1.X,
(float.IsNaN(newV1.Y)) ? 0 : newV1.Y);
newV2 = new Vector2(
(float.IsNaN(newV2.X)) ? 0 : newV2.X,
(float.IsNaN(newV2.Y)) ? 0 : newV2.Y);
AddForce(newV1);
Object.Physik.AddForce(newV2);
}
bool checkCollidingObjects(IPhysicsObject obj1, IPhysicsObject obj2)
{
if (CollidingObjects.Count > 0)
{
int a = CollidingObjects.FindIndex(x => (x[0] == obj1 && x[1] == obj2) ||
(x[1] == obj1 && x[0] == obj2));
return a != -1;
}
return false;
}