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;
}
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
What I'm trying to do is select a specific triangle on any given mesh, so I'm using the Barycentric algorithm. However, as seen here, there are some strange behaviors at certain locations and always an unwanted second triangle. Best guess, it has something to do with inaccurate float precision of the Vector3.
Here is the Script I'm Currently Using:
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class SpecificTriangleIndex : MonoBehaviour
{
public Camera m_camera;
public LayerMask layermask;
public Vector3[] vertices;
public Vector3[] normals;
public int[] triangles;
public List<int> triangleIndices = new List<int>(); ///Returned Triangle Indices -- What it thinks is selected.
public List<Vector3> baryAllens = new List<Vector3>(); ///Position of Verticies of Returned Triangles
private RaycastHit hit;
private Transform objectHit;
private Vector3 bary;
void Update()
{
if (Input.GetMouseButton(0))
{
EobardThawne();
Raycast();
}
}
void Raycast()
{
Ray ray = m_camera.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out hit, layermask))
{
objectHit = hit.transform;
MeshFilter meshFilter = hit.transform.gameObject.GetComponent<MeshFilter>();
Mesh mesh = meshFilter.sharedMesh;
vertices = mesh.vertices;
normals = mesh.normals;
triangles = mesh.triangles;
Vector3 p = transform.InverseTransformPoint(hit.point);
for (int i = 0; i < triangles.Length; i += 3)
{
Vector3 a = vertices[triangles[i]];
Vector3 b = vertices[triangles[i + 1]];
Vector3 c = vertices[triangles[i + 2]];
bary = GetBarycentric(a, b, c, p);
if (InTriangle(bary))
{
triangleIndices.Add(i / 3);
baryAllens.Add(a + objectHit.position);
baryAllens.Add(b + objectHit.position);
baryAllens.Add(c + objectHit.position);
}
}
for (int i = 0; i < baryAllens.Count; i += 3)
{
Vector3 v0 = baryAllens[i];
Vector3 v1 = baryAllens[i + 1];
Vector3 v2 = baryAllens[i + 2];
Debug.DrawLine(v0, v1, Color.green);
Debug.DrawLine(v1, v2, Color.green);
Debug.DrawLine(v2, v0, Color.green);
}
}
}
Vector3 GetBarycentric(Vector2 v1, Vector2 v2, Vector2 v3, Vector2 p)
{
Vector3 B = new Vector3();
B.x = ((v2.y - v3.y) * (p.x - v3.x) + (v3.x - v2.x) * (p.y - v3.y)) /
((v2.y - v3.y) * (v1.x - v3.x) + (v3.x - v2.x) * (v1.y - v3.y));
B.y = ((v3.y - v1.y) * (p.x - v3.x) + (v1.x - v3.x) * (p.y - v3.y)) /
((v3.y - v1.y) * (v2.x - v3.x) + (v1.x - v3.x) * (v2.y - v3.y));
B.z = 1 - B.x - B.y;
return B;
}
bool InTriangle(Vector3 barycentric)
{
return (barycentric.x >= 0.0f) && (barycentric.x <= 1.0f)
&& (barycentric.y >= 0.0f) && (barycentric.y <= 1.0f)
&& (barycentric.z >= 0.0f); //(barycentric.z <= 1.0f)
}
private void OnDrawGizmos()
{
Gizmos.color = Color.yellow;
Gizmos.DrawSphere(hit.point, .01f);
Gizmos.color = Color.cyan;
foreach (int i in triangleIndices)
{
Gizmos.DrawSphere(FindCenter(i * 3), .01f);
Debug.DrawLine(hit.point, FindCenter(i * 3), Color.red);
}
}
public void EobardThawne()
{
triangleIndices.Clear();
baryAllens.Clear();
}
public Vector3 FindCenter(int i)
{
Vector3 v0 = transform.TransformPoint(vertices[triangles[i]]);
Vector3 v1 = transform.TransformPoint(vertices[triangles[i + 1]]);
Vector3 v2 = transform.TransformPoint(vertices[triangles[i + 2]]);
Vector3 center = (v0 + v1 + v2) / 3;
return center;
}
}
Your triangles are duplicated because you fully ignore the Z axis!
You do all your triangle math assuming 2D space and only using Vector2!
=> Of course you get a second triangle since on your round objects there will always be two triangles overlapping along the Z axis!
You will have to use an actual 3D "point in triangle" test like e.g. the one from this thread
bool PointInTriangle(Vector3 a, Vector3 b, Vector3 c, Vector3 p)
{
Vector3 d, e;
double w1, w2;
d = b - a;
e = c - a;
if (Mathf.Approximately(e.y, 0))
{
e.y = 0.0001f;
}
w1 = (e.x * (a.y - p.y) + e.y * (p.x - a.x)) / (d.x * e.y - d.y * e.x);
w2 = (p.y - a.y - w1 * d.y) / e.y;
return (w1 >= 0f) && (w2 >= 0.0) && ((w1 + w2) <= 1.0);
}
And then personally I would add some proper classes instead of arrays and do e.g.
public class SpecificTriangleIndex : MonoBehaviour
{
public Camera m_camera;
public LayerMask layermask;
public class HitInfo
{
public Vector3 Point;
public Triangle Triangle = new Triangle();
}
public class Triangle
{
public Vector3 A;
public Vector3 B;
public Vector3 C;
public Vector3 Center => (A + B + C) / 3f;
}
private HitInfo currenHit;
private void Update()
{
if (Input.GetMouseButton(0))
{
Raycast();
}
}
private void Raycast()
{
var ray = m_camera.ScreenPointToRay(Input.mousePosition);
if (Physics.Raycast(ray, out var hit, float.PositiveInfinity, layermask))
{
var meshFilter = hit.transform.gameObject.GetComponent<MeshFilter>();
var mesh = meshFilter.sharedMesh;
var vertices = mesh.vertices;
var triangles = mesh.triangles;
// Here you need to go into the local space of the hit not yourself!
var p = hit.transform.InverseTransformPoint(hit.point);
for (var i = 0; i < triangles.Length; i += 3)
{
var a = vertices[triangles[i]];
var b = vertices[triangles[i + 1]];
var c = vertices[triangles[i + 2]];
if (PointInTriangle(a, b, c, p))
{
if (currenHit == null)
{
currenHit = new HitInfo();
}
currenHit.Point = hit.point;
// as before you also want to convert back using the hit transform, not your own
currenHit.Triangle.A = hit.transform.TransformPoint(a);
currenHit.Triangle.B = hit.transform.TransformPoint(b);
currenHit.Triangle.C = hit.transform.TransformPoint(c);
// we only want one triangle anyway so we can skip the remaining triangles
break;
}
}
}
else
{
currenHit = null;
}
}
private static bool PointInTriangle(Vector3 a, Vector3 b, Vector3 c, Vector3 p)
{
var d = b - a;
var e = c - a;
if (Mathf.Approximately(e.y, 0))
{
e.y = 0.0001f;
}
double w1 = (e.x * (a.y - p.y) + e.y * (p.x - a.x)) / (d.x * e.y - d.y * e.x);
var w2 = (p.y - a.y - w1 * d.y) / e.y;
return (w1 >= 0f) && (w2 >= 0.0) && ((w1 + w2) <= 1.0);
}
private void OnDrawGizmos()
{
// only draw if there is a hit
if (currenHit == null) return;
Gizmos.color = Color.yellow;
Gizmos.DrawSphere(currenHit.Point, .01f);
Gizmos.color = Color.cyan;
Gizmos.DrawSphere(currenHit.Triangle.Center, .01f);
Gizmos.color = Color.red;
Gizmos.DrawLine(currenHit.Point, currenHit.Triangle.Center);
Gizmos.color = Color.green;
Gizmos.DrawLine(currenHit.Triangle.A, currenHit.Triangle.B);
Gizmos.DrawLine(currenHit.Triangle.B, currenHit.Triangle.C);
Gizmos.DrawLine(currenHit.Triangle.C, currenHit.Triangle.A);
}
}
Maybe hard to see but this is how it looks like now
enter image description here
Like the attached image,
I don't want the red circle following the green mouse position to go out of the fan-shaped area.
So I wrote the code as below.
However, it is not working properly.
What's the problem?
float GetDegree(Vector3 start, Vector3 end)
{
var dx = end.x - start.x;
var dz = end.z - start.z;
var degree = Mathf.Atan2(dx, dz) * Mathf.Rad2Deg;
return degree;
}
void update()
{
float range = 5;
float centerAngle = 30;
Vector3 centerPosition = new vector3 (0, 0, 0);
Vector3 mousePosition = Input.mousePosition
float R = Vector3.Distance(centerPosition, mousePosition);
float T = GetDegree(centerPosition, mousePosition);
float tmin = Mathf.Abs(90f - (centerAngle) * 0.5f) * (T >= 0 ? 1F : -1F);
float tmax = (T >= 0 ? centerAngle : -centerAngle) + tmin;
float r = range;
float R1 = Mathf.Min(R, r);
float T1 = Mathf.Clamp(T, tmin, tmax);
float rcost = R1 * Mathf.Cos(T1);
float rsint = R1 * Mathf.Sin(T1);
Vector3 result = new vector3 (rcost, 0f, rsint);
}
I'm currently making my Boss AI perform a jump attack against the player, the AI use both navmesh and character controller for movment (navmash only for pathfinding), but I'm having a hard time trying to move the AI to the designated position. here is my code:
CharacterController chara;
[SerializeField]
Transform playerTran;
[SerializeField]
float gravity = 3.8f;
[SerializeField]
float jumpForce = 50F;
Vector3 moveVector = Vector3.zero;
[SerializeField]
Transform jumpCheck;
[SerializeField]
Transform jumpPos;
// Use this for initialization
void Start ()
{
chara = GetComponent<CharacterController>();
playerTran = GameObject.FindGameObjectWithTag("Player").GetComponent<Transform>();
}
// Update is called once per frame
void Update ()
{
chara.Move(moveVector);
if (chara.isGrounded)
{
transform.LookAt(playerTran);
}
moveVector -= Vector3.up * gravity * Time.deltaTime;
if (Input.GetKeyUp(KeyCode.J))
{
jumpCheck.position = new Vector3(playerTran.position.x, 0, playerTran.position.z);
float angle = findAngle( playerTran.position.x - transform.position.x , playerTran.position.z - transform.position.z);
Vector3 _toJumpPos = MakeJumpCricle(playerTran.position , jumpCheck.localScale.x/2, angle);
jumpPos.position = new Vector3(_toJumpPos.x, 0, _toJumpPos.z);
moveVector = Vector3.up * jumpForce * Time.deltaTime;
}
}
float findAngle(float x, float y)
{
float value;
value = (float)((Mathf.Atan2(x, y) / Mathf.PI) * 180);
if (value < 0)
{
value += 360;
}
Debug.Log(value);
return value;
}
Vector3 MakeJumpCricle( Vector3 center, float radius, float angle)
{
Vector3 pos = Vector3.zero;
pos.x = center.x - radius * Mathf.Sin(angle * Mathf.Deg2Rad);
pos.y = 0;
pos.z = center.z - radius * Mathf.Cos(angle * Mathf.Deg2Rad);
return pos;
}
I want to move the AI to the jumpPos with both forward and up vectors but I'm not sure to do this.
visualization of the code
code visualization
I found a good solution, I used a Bezier Curves to generate a path
posting it here so other might find it helpful.
refrence link
http://www.theappguruz.com/blog/bezier-curve-in-games
public LineRenderer jumpLine;
private int numberOfPoint = 50;
[SerializeField]
List<Vector3> pointPositions = new List<Vector3>();
CharacterController chara;
[SerializeField]
Transform playerTran;
[SerializeField]
float gravity = 3.8f;
[SerializeField]
float jumpForce = 50F;
Vector3 moveVector = Vector3.zero;
[SerializeField]
Transform jumpCheck;
[SerializeField]
Transform jumpPos;
[SerializeField]
Transform jumpHightOne;
bool movejump;
Vector3 pZero;
Vector3 pOne;
float time;
// Use this for initialization
void Start ()
{
chara = GetComponent<CharacterController>();
playerTran = GameObject.FindGameObjectWithTag("Player").GetComponent<Transform>();
jumpLine.positionCount = numberOfPoint;
jumpLine.gameObject.SetActive(false);
}
// Update is called once per frame
void Update ()
{
chara.Move(moveVector);
if (chara.isGrounded)
{
transform.LookAt(playerTran);
}
moveVector -= Vector3.up * gravity * Time.deltaTime;
if (Input.GetKeyUp(KeyCode.J))
{
jumpCheck.position = new Vector3(playerTran.position.x, 0, playerTran.position.z);
float angle = findAngle( playerTran.position.x - transform.position.x , playerTran.position.z - transform.position.z);
Vector3 _toJumpPos = MakeJumpCricle(playerTran.position , jumpCheck.localScale.x/2, angle);
Vector3 middlePoint = GetTheMiddlePoints(transform.position, playerTran.position, 0.5f);
jumpHightOne.position = new Vector3(middlePoint.x, jumpHightOne.position.y, middlePoint.z);
jumpPos.position = new Vector3(_toJumpPos.x, 0, _toJumpPos.z);
//moveVector = Vector3.up * jumpForce * Time.deltaTime;
//movejump = true;
DrawLinerBezierCurves();
}
if (movejump)
{
//MoveTowardsTarget(jumpPos.position);
}
}
Vector3 GetTheMiddlePoints(Vector3 origin, Vector3 destination, float middlepointfactor)
{
return (destination - origin) * middlepointfactor + origin;
// (destination - origin) * 0.5f;
}
float findAngle(float x, float y)
{
float value;
value = (float)((Mathf.Atan2(x, y) / Mathf.PI) * 180);
if (value < 0)
{
value += 360;
}
//Debug.Log(value);
return value;
}
Vector3 MakeJumpCricle( Vector3 center, float radius, float angle)
{
Vector3 pos = Vector3.zero;
pos.x = center.x - radius * Mathf.Sin(angle * Mathf.Deg2Rad);
pos.y = 0;
pos.z = center.z - radius * Mathf.Cos(angle * Mathf.Deg2Rad);
return pos;
}
void MoveTowardsTarget(Vector3 targetPostios)
{
var offset = targetPostios - transform.position;
if (offset.magnitude > .1f)
{
offset = offset.normalized * 15;
chara.Move(offset * Time.deltaTime);
}
else
{
movejump = false;
}
}
// line Bezier Curves
Vector3 CalculateBezierCurvesPoints(float t , Vector3 p0, Vector3 p1 )
{
return p0 + t * (p1 - p0);
// P = P0 + t(P1 – P0) , 0 < t < 1
}
// line Bezier Curves
Vector3 CalculateBezierCurvesPoints(float t, Vector3 p0, Vector3 p1, Vector3 p2)
{
float u = 1 - t;
float uu = u * u;
float tt = t * t;
Vector3 p = uu * p0;
p = p + 2 * u * t * p1;
p = p + tt * p2;
return p;
//P = (1-t)^2 P0 + 2 (1-t) t P1 + t^2 P2 , 0 < t < 1
// uu u tt
// uu * p0 + 2 * u * t * p1 + tt * p2
}
void DrawLinerBezierCurves()
{
if (pointPositions.Count > 0)
{
pointPositions.Clear();
jumpLine.positionCount = 0;
jumpLine.positionCount = numberOfPoint;
}
for (int i = 1; i < numberOfPoint + 1; i++)
{
float t = i / (float) numberOfPoint;
if (!jumpLine.gameObject.activeInHierarchy)
{
jumpLine.gameObject.SetActive(true);
}
pointPositions.Add(CalculateBezierCurvesPoints(t, transform.position,jumpHightOne.position, jumpPos.position));
jumpLine.SetPosition(i - 1, pointPositions[i - 1]);
}
}
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;
}