public float GetJointTranslation()
{
Body b1 = m_bodyA;
Body b2 = m_bodyB;
Vector2 p1 = b1.GetWorldPoint(m_localAnchorA);
Vector2 p2 = b2.GetWorldPoint(m_localAnchorB);
Vector2 d = p2 - p1;
Vector2 axis = b1.GetWorldVector(m_localXAxisA);
return(Vector2.Dot(d, axis));
}
public bool intersects(Primitives primitive)
{
if (primitive.PrimitivesType == PrimitivesType.Circle)
{
// Richtingsvector van het middelpunt cirkel naar huidige pixel.
Point pos = new Point(primitive.centerX, primitive.centerY);
float cX = pos.X - O.X;
float cY = pos.Y - O.Y;
Vector2 c = new Vector2(cY, cX);
// Het component van vector c op de richtingsvector naar het lichtpunt.
// variabele t uit de sheets L5;43 hernoemd naar m, omdat t al wordt gebruikt als ray.t
float m = Vector2.Dot(c, D);
Vector2 q = c - m * D;
// De lengte van vector Q.
float p2 = Vector2.Dot(q, q);
// In dit geval zijn er geen snijpunten:
double radius2 = Math.Pow(primitive.radius, 2);
if (p2 > radius2)
{
return(false);
}
// Het punt wanneer de ray de cirkel raakt.
m -= (float)Math.Sqrt(radius2 - p2);
// Als
if ((m < t) && (m > 0))
{
t = m;
return(true);
}
}
// Als er geen primitives bestaan dan is er ook geen intersectie.
return(false);
//hier berekenen of de ray de gegeven primitive snijdt
//ik return nu true om foutmeldingen te voorkomen, dit moet nog veranderd
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
public float JointSpeed()
{
Body b1 = m_bodyA;
Body b2 = m_bodyB;
Vector2 r1 = Vector2.Transform(m_localAnchorA - b1.GetLocalCenter(), b1.GetTransform().q);
Vector2 r2 = Vector2.Transform(m_localAnchorB - b2.GetLocalCenter(), b2.GetTransform().q);
Vector2 p1 = b1.m_sweep.c + r1;
Vector2 p2 = b2.m_sweep.c + r2;
Vector2 d = p2 - p1;
Vector2 axis = b1.GetWorldVector(m_localXAxisA);
Vector2 v1 = b1.m_linearVelocity;
Vector2 v2 = b2.m_linearVelocity;
float w1 = b1.m_angularVelocity;
float w2 = b2.m_angularVelocity;
return(Vector2.Dot(d, Vectex.Cross(w1, axis)) +
Vector2.Dot(axis, v2 + Vectex.Cross(w2, r2) - v1 - Vectex.Cross(w1, r1)));
}
[MethodImpl(MethodImplOptions.AggressiveInlining)] public static FLOAT Dot(VECTOR a, VECTOR b)
{
return(VECTOR.Dot(a, b));
}
public static float DotProduct(Vector2 left, Vector2 right) => SysVector2.Dot(left._value, right._value);
