| public GetWorldVector ( System.Vector2 localVector ) : System.Vector2 | ||
| localVector | System.Vector2 | A vector fixed in the body. |
| return | System.Vector2 | |
public float GetJointTranslation()
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 worldPoint = body.GetWorldPoint(this._localAnchor1);
Vec2 worldPoint2 = body2.GetWorldPoint(this._localAnchor2);
Vec2 a = worldPoint2 - worldPoint;
Vec2 worldVector = body.GetWorldVector(this._localXAxis1);
return(Vec2.Dot(a, worldVector));
}
/// <summary>
/// Get the current joint translation, usually in meters.
/// </summary>
public float GetJointTranslation()
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 p1 = b1.GetWorldPoint(_localAnchor1);
Vector2 p2 = b2.GetWorldPoint(_localAnchor2);
Vector2 d = p2 - p1;
Vector2 axis = b1.GetWorldVector(_localXAxis1);
float translation = Vector2.Dot(d, axis);
return(translation);
}
public float GetJointSpeed()
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
Vec2 v = body._sweep.C + vec;
Vec2 v2 = body2._sweep.C + vec2;
Vec2 a = v2 - v;
Vec2 worldVector = body.GetWorldVector(this._localXAxis1);
Vec2 linearVelocity = body._linearVelocity;
Vec2 linearVelocity2 = body2._linearVelocity;
float angularVelocity = body._angularVelocity;
float angularVelocity2 = body2._angularVelocity;
return(Vec2.Dot(a, Vec2.Cross(angularVelocity, worldVector)) + Vec2.Dot(worldVector, linearVelocity2 + Vec2.Cross(angularVelocity2, vec2) - linearVelocity - Vec2.Cross(angularVelocity, vec)));
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
public float GetJointSpeed()
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.C + r1;
Vector2 p2 = b2._sweep.C + r2;
Vector2 d = p2 - p1;
Vector2 axis = b1.GetWorldVector(_localXAxis1);
Vector2 v1 = b1._linearVelocity;
Vector2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
float speed = Vector2.Dot(d, axis.CrossScalarPreMultiply(w1)) + Vector2.Dot(axis, v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1));
return(speed);
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
public float GetJointSpeed()
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 r1 = Box2DX.Common.Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DX.Common.Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 d = p2 - p1;
Vec2 axis = b1.GetWorldVector(_localXAxis1);
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
float speed = Vec2.Dot(d, Vec2.Cross(w1, axis)) + Vec2.Dot(axis, v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1));
return(speed);
}
/// <summary>
/// Add a spring force
/// </summary>
void AddSpringForce(Body bA, Vec2 localA, Body bB, Vec2 localB, float k, float friction, float desiredDist)
{
Vec2 pA = bA.GetWorldPoint(localA);
Vec2 pB = bB.GetWorldPoint(localB);
Vec2 diff = pB - pA;
//Find velocities of attach points
Vec2 vA = bA.GetLinearVelocity() - Vec2.Cross(bA.GetWorldVector(localA), bA.GetAngularVelocity());
Vec2 vB = bB.GetLinearVelocity() - Vec2.Cross(bB.GetWorldVector(localB), bB.GetAngularVelocity());
Vec2 vdiff = vB - vA;
float dx = diff.Normalize(); //normalizes diff and puts length into dx
float vrel = vdiff.X * diff.X + vdiff.Y * diff.Y;
float forceMag = -k * (dx - desiredDist) - friction * vrel;
diff *= forceMag; // diff *= forceMag
bB.ApplyForce(diff, bA.GetWorldPoint(localA));
diff *= -1.0f;
bA.ApplyForce(diff, bB.GetWorldPoint(localB));
}