Here are the examples of the csharp api System.Math.Abs(float) taken from open source projects. By voting up you can indicate which examples are most useful and appropriate.
1741 Examples
19
Source : WheelShape.cs
with The Unlicense
from aeroson
with The Unlicense
from aeroson
internal void UpdateSpin(float dt)
{
if (wheel.Hreplacedupport && !(wheel.brake.IsBraking && FreezeWheelsWhileBraking))
{
//On the ground, not braking.
spinVelocity = wheel.drivingMotor.RelativeVelocity / Radius;
}
else if (wheel.Hreplacedupport && wheel.brake.IsBraking && FreezeWheelsWhileBraking)
{
//On the ground, braking
float deceleratedValue = 0;
if (spinVelocity > 0)
deceleratedValue = Math.Max(spinVelocity - brakeFreezeWheelDeceleration * dt, 0);
else if (spinVelocity < 0)
deceleratedValue = Math.Min(spinVelocity + brakeFreezeWheelDeceleration * dt, 0);
spinVelocity = wheel.drivingMotor.RelativeVelocity / Radius;
if (Math.Abs(deceleratedValue) < Math.Abs(spinVelocity))
spinVelocity = deceleratedValue;
}
else if (!wheel.Hreplacedupport && wheel.drivingMotor.TargetSpeed != 0)
{
//Airborne and accelerating, increase spin velocity.
float maxSpeed = Math.Abs(wheel.drivingMotor.TargetSpeed) / Radius;
spinVelocity = MathHelper.Clamp(spinVelocity + Math.Sign(wheel.drivingMotor.TargetSpeed) * airborneWheelAcceleration * dt, -maxSpeed, maxSpeed);
}
else if (!wheel.Hreplacedupport && wheel.Brake.IsBraking)
{
//Airborne and braking
if (spinVelocity > 0)
spinVelocity = Math.Max(spinVelocity - brakeFreezeWheelDeceleration * dt, 0);
else if (spinVelocity < 0)
spinVelocity = Math.Min(spinVelocity + brakeFreezeWheelDeceleration * dt, 0);
}
else if (!wheel.Hreplacedupport)
{
//Just idly slowing down.
if (spinVelocity > 0)
spinVelocity = Math.Max(spinVelocity - airborneWheelDeceleration * dt, 0);
else if (spinVelocity < 0)
spinVelocity = Math.Min(spinVelocity + airborneWheelDeceleration * dt, 0);
}
spinAngle += spinVelocity * dt;
}
19
Source : WheelSlidingFriction.cs
with The Unlicense
from aeroson
with The Unlicense
from aeroson
internal void PreStep(float dt)
{
vehicleEnreplacedy = wheel.Vehicle.Body;
supportEnreplacedy = wheel.SupportingEnreplacedy;
supportIsDynamic = supportEnreplacedy != null && supportEnreplacedy.isDynamic;
Vector3.Cross(ref wheel.worldForwardDirection, ref wheel.normal, out slidingFrictionAxis);
float axisLength = slidingFrictionAxis.LengthSquared();
//Safety against bad cross product
if (axisLength < Toolbox.BigEpsilon)
{
Vector3.Cross(ref wheel.worldForwardDirection, ref Toolbox.UpVector, out slidingFrictionAxis);
axisLength = slidingFrictionAxis.LengthSquared();
if (axisLength < Toolbox.BigEpsilon)
{
Vector3.Cross(ref wheel.worldForwardDirection, ref Toolbox.RightVector, out slidingFrictionAxis);
}
}
slidingFrictionAxis.Normalize();
linearAX = slidingFrictionAxis.X;
linearAY = slidingFrictionAxis.Y;
linearAZ = slidingFrictionAxis.Z;
//angular A = Ra x N
angularAX = (wheel.ra.Y * linearAZ) - (wheel.ra.Z * linearAY);
angularAY = (wheel.ra.Z * linearAX) - (wheel.ra.X * linearAZ);
angularAZ = (wheel.ra.X * linearAY) - (wheel.ra.Y * linearAX);
//Angular B = N x Rb
angularBX = (linearAY * wheel.rb.Z) - (linearAZ * wheel.rb.Y);
angularBY = (linearAZ * wheel.rb.X) - (linearAX * wheel.rb.Z);
angularBZ = (linearAX * wheel.rb.Y) - (linearAY * wheel.rb.X);
//Compute inverse effective mreplaced matrix
float entryA, entryB;
//these are the transformed coordinates
float tX, tY, tZ;
if (vehicleEnreplacedy.isDynamic)
{
tX = angularAX * vehicleEnreplacedy.inertiaTensorInverse.M11 + angularAY * vehicleEnreplacedy.inertiaTensorInverse.M21 + angularAZ * vehicleEnreplacedy.inertiaTensorInverse.M31;
tY = angularAX * vehicleEnreplacedy.inertiaTensorInverse.M12 + angularAY * vehicleEnreplacedy.inertiaTensorInverse.M22 + angularAZ * vehicleEnreplacedy.inertiaTensorInverse.M32;
tZ = angularAX * vehicleEnreplacedy.inertiaTensorInverse.M13 + angularAY * vehicleEnreplacedy.inertiaTensorInverse.M23 + angularAZ * vehicleEnreplacedy.inertiaTensorInverse.M33;
entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + vehicleEnreplacedy.inverseMreplaced;
}
else
entryA = 0;
if (supportIsDynamic)
{
tX = angularBX * supportEnreplacedy.inertiaTensorInverse.M11 + angularBY * supportEnreplacedy.inertiaTensorInverse.M21 + angularBZ * supportEnreplacedy.inertiaTensorInverse.M31;
tY = angularBX * supportEnreplacedy.inertiaTensorInverse.M12 + angularBY * supportEnreplacedy.inertiaTensorInverse.M22 + angularBZ * supportEnreplacedy.inertiaTensorInverse.M32;
tZ = angularBX * supportEnreplacedy.inertiaTensorInverse.M13 + angularBY * supportEnreplacedy.inertiaTensorInverse.M23 + angularBZ * supportEnreplacedy.inertiaTensorInverse.M33;
entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + supportEnreplacedy.inverseMreplaced;
}
else
entryB = 0;
velocityToImpulse = -1 / (entryA + entryB); //Softness?
//Compute friction.
//Which coefficient? Check velocity.
if (Math.Abs(RelativeVelocity) < staticFrictionVelocityThreshold)
blendedCoefficient = frictionBlender(staticCoefficient, wheel.supportMaterial.staticFriction, false, wheel);
else
blendedCoefficient = frictionBlender(kineticCoefficient, wheel.supportMaterial.kineticFriction, true, wheel);
}
19
Source : Quaternion.cs
with The Unlicense
from aeroson
with The Unlicense
from aeroson
public static float GetAngleFromQuaternion(ref Quaternion q)
{
float qw = Math.Abs(q.W);
if (qw > 1)
return 0;
return 2 * (float)Math.Acos(qw);
}
19
Source : Simulator.cs
with MIT License
from aillieo
with MIT License
from aillieo
private void OA(Agent agent, float timeStep)
{
agent.collisions.Clear();
Vector2 expected = agent.moveWithOA;
Vector2 moveWithOADir = expected;
Vector2 rightHand = Vector2.Perpendicular(expected);
float avoidLeft = 0;
float avoidRight = 0;
foreach (var neighbor in agent.neighbors)
{
if (neighbor != agent)
{
Vector2 posRelative = agent.position - neighbor.position;
if (Vector2.Dot(expected, - posRelative) > 0)
{
float distanceSq = posRelative.SqrMagnitude();
float minDistAllow = (agent.radius + neighbor.radius) * (1 + config.spaceFactor);
float distanceIgnore = agent.speed * timeStep * config.distanceIgnoreFactor + minDistAllow;
if(distanceSq > distanceIgnore * distanceIgnore)
{
continue;
}
// 在垂直方向上(右手边)的投影
float distHorizontal = Vector2.Dot(posRelative, rightHand);
// 方向会冲突 && 距离足够近 (向前和向右的投影 都小于半径之和)
if (Vector2.Dot(posRelative, moveWithOADir) < minDistAllow && Math.Abs(distHorizontal) < minDistAllow)
{
//if (Math.Abs(distHorizontal) <= float.Epsilon)
//{
// Debug.LogError("正前方");
//}
float dist = (float)Math.Sqrt(distanceSq);
float hFactor = config.horizontalFactor;
if (distHorizontal > 0)
{
avoidRight += (minDistAllow - distHorizontal) * hFactor * dist;
moveWithOADir += avoidRight * rightHand;
moveWithOADir.Normalize();
}
else
{
avoidLeft += (minDistAllow + distHorizontal) * hFactor * dist;
moveWithOADir -= avoidLeft * rightHand;
moveWithOADir.Normalize();
}
rightHand = Vector2.Perpendicular(moveWithOADir);
agent.collisions.Add(neighbor);
}
}
}
}
var oa = Vector2.Perpendicular(expected) * (avoidRight - avoidLeft);
moveWithOADir = (expected + oa).normalized;
float mag = Math.Min(agent.speed * timeStep, Vector2.Distance(agent.goal, agent.position));
agent.moveWithOA = moveWithOADir * mag;
}
19
Source : Simulator.cs
with MIT License
from aillieo
with MIT License
from aillieo
private static void MoveAgent(Agent agent, float timeStep)
{
foreach (var c in agent.collisions)
{
// 如果按照move移动之后 会碰撞 那么截断
Vector2 relativeMove = agent.moveWithOA - c.moveWithOA;
Vector2 relativePos = agent.position - c.position;
float relativeDist = relativePos.magnitude - agent.radius - c.radius;
relativePos.Normalize();
relativePos *= relativeDist;
float project = Vector2.Dot(relativePos, relativeMove);
if (project > 0)
{
//Debug.LogError("collision");
agent.moveWithOA = agent.moveWithOA.normalized * Math.Min(agent.moveWithOA.magnitude, Math.Abs(project));
}
}
agent.position += agent.moveWithOA;
}
19
Source : NumericManipulator.cs
with MIT License
from akaskela
with MIT License
from akaskela
public bool AlmostEquals(float a, float b, float epsilon = FLOAT_EPSILON)
{
// ReSharper disable CompareOfFloatsByEqualityOperator
if (a == b)
{
return true;
}
// ReSharper restore CompareOfFloatsByEqualityOperator
return (System.Math.Abs(a - b) < epsilon);
}
19
Source : QuadComponent.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public virtual bool PrepareQuad(RectF cullRect, out QuadSpec quad)
{
float entX = Core.SnapToPixel(enreplacedy.Position.X);
float entY = Core.SnapToPixel(enreplacedy.Position.Y);
float entZ = 0;//entY+entX*0.0001f; //TODO
float S = 4; // snap positions
float M = S / (float)Math.PI;
float visualRotation = (float)Math.Floor((enreplacedy.Rotation + Math.PI / S / 2) * M) / M;
//visualRotation = enreplacedy.Rotation;
bool flipX = quadData.flipX;
float scaleX = quadData.atlasTile.width * Core.ATLAS_TO_WORLD * quadData.scale.X;
float scaleY = quadData.atlasTile.height * Core.ATLAS_TO_WORLD * quadData.scale.Y;
float originX = quadData.origin.X;
float originY = quadData.origin.Y;
float topShear = quadData.topShear;
float rightShear = quadData.rightShear;
Vector3 corner0;
Vector3 corner1;
Vector3 corner2;
Vector3 corner3;
if (Math.Abs(visualRotation) > 0.001f)
{
float rotSin = (float)Math.Sin(visualRotation);
float rotCos = (float)Math.Cos(visualRotation);
float ooX = rotCos * originX - rotSin * originY;
float ooY = rotSin * originX + rotCos * originY;
corner0 = new Vector3(entX - ooX, entY - ooY, 0);
corner1 = new Vector3(entX - rotSin * scaleY - ooX + topShear, entY + rotCos * scaleY - ooY, 0);
corner2 = new Vector3(entX + (rotCos * scaleX - rotSin * scaleY - ooX) + topShear, entY + (rotCos * scaleY + rotSin * scaleX) - ooY + rightShear, 0);
corner3 = new Vector3(entX + rotCos * scaleX - ooX, entY + rotSin * scaleX - ooY + rightShear, 0);
}
else
{
corner0 = new Vector3(entX - originX, entY - originY, entZ);
corner1 = new Vector3(entX - originX + topShear, entY + scaleY - originY, entZ);
corner2 = new Vector3(entX + scaleX - originX + topShear, entY + scaleY - originY + rightShear, entZ);
corner3 = new Vector3(entX + scaleX - originX, entY - originY + rightShear, entZ);
}
//narrowphase culling
RectF aabb = MathUtils.AABBFromCorners(corner0, corner1, corner2, corner3);
if (!cullRect.Intersects(aabb))
{
quad = new QuadSpec();
return false;
}
Dbg.AddDebugRect(aabb, Color.Yellow, 0.5f);
//TODO: get verts positions and uv from quaddata method?
Vector2 corner0uv = new Vector2(quadData.atlasTile.X, quadData.atlasTile.Bottom);
Vector2 corner1uv = new Vector2(quadData.atlasTile.X, quadData.atlasTile.Y);
Vector2 corner2uv = new Vector2(quadData.atlasTile.Right, quadData.atlasTile.Y);
Vector2 corner3uv = new Vector2(quadData.atlasTile.Right, quadData.atlasTile.Bottom);
if (flipX)
{
GeneralUtils.Swap(ref corner0uv, ref corner3uv);
GeneralUtils.Swap(ref corner1uv, ref corner2uv);
}
quad = new QuadSpec(
corner0,corner1,corner2,corner3,
corner0uv,corner1uv,corner2uv,corner3uv,
Color.White,Color.White,Color.White,Color.White
);
return true;
}
19
Source : RectF.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public bool IsSimilarTo(RectF other, float tolerance)
{
return Math.Abs(X - other.X) < tolerance &&
Math.Abs(Y - other.Y) < tolerance &&
Math.Abs(width - other.width) < tolerance &&
Math.Abs(height - other.height) < tolerance;
}
19
Source : TimeOfImpact.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input)
{
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++TOICalls;
output = new TOIOutput();
output.State = TOIOutputState.Unknown;
output.T = input.TMax;
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float tMax = input.TMax;
float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius;
float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
const float tolerance = 0.25f * Settings.LinearSlop;
Debug.replacedert(target > tolerance);
float t1 = 0.0f;
const int k_maxIterations = 20;
int iter = 0;
// Prepare input for distance query.
_distanceInput = _distanceInput ?? new DistanceInput();
_distanceInput.ProxyA = input.ProxyA;
_distanceInput.ProxyB = input.ProxyB;
_distanceInput.UseRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for (; ; )
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
_distanceInput.TransformA = xfA;
_distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
SimplexCache cache;
Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.Distance <= 0.0f)
{
// Failure!
output.State = TOIOutputState.Overlapped;
output.T = 0.0f;
break;
}
if (distanceOutput.Distance < target + tolerance)
{
// Victory!
output.State = TOIOutputState.Touching;
output.T = t1;
break;
}
SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1);
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of vertices.
bool done = false;
float t2 = tMax;
int pushBackIter = 0;
for (; ; )
{
// Find the deepest point at t2. Store the witness point indices.
int indexA, indexB;
float s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
output.State = TOIOutputState.Seperated;
output.T = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
float s1 = SeparationFunction.Evaluate(indexA, indexB, t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
if (s1 < target - tolerance)
{
output.State = TOIOutputState.Failed;
output.T = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
output.State = TOIOutputState.Touching;
output.T = t1;
done = true;
break;
}
// Compute 1D root of: f(x) - target = 0
int rooreplacederCount = 0;
float a1 = t1, a2 = t2;
for (; ; )
{
// Use a mix of the secant rule and bisection.
float t;
if ((rooreplacederCount & 1) != 0)
{
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
++rooreplacederCount;
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++TOIRooreplaceders;
float s = SeparationFunction.Evaluate(indexA, indexB, t);
if (Math.Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = t;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = t;
s1 = s;
}
else
{
a2 = t;
s2 = s;
}
if (rooreplacederCount == 50)
{
break;
}
}
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
TOIMaxRooreplaceders = Math.Max(TOIMaxRooreplaceders, rooreplacederCount);
++pushBackIter;
if (pushBackIter == Settings.MaxPolygonVertices)
{
break;
}
}
++iter;
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
++TOIIters;
if (done)
{
break;
}
if (iter == k_maxIterations)
{
// Root finder got stuck. Semi-victory.
output.State = TOIOutputState.Failed;
output.T = t1;
break;
}
}
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
TOIMaxIters = Math.Max(TOIMaxIters, iter);
}
19
Source : SimplifyTools.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public static Vertices ReduceByArea(Vertices vertices, float areaTolerance)
{
//From physics2d.net
if (vertices.Count <= 3)
return vertices;
if (areaTolerance < 0)
throw new ArgumentOutOfRangeException("areaTolerance", "must be equal to or greater than zero.");
Vertices simplified = new Vertices(vertices.Count);
Vector2 v3;
Vector2 v1 = vertices[vertices.Count - 2];
Vector2 v2 = vertices[vertices.Count - 1];
areaTolerance *= 2;
for (int i = 0; i < vertices.Count; ++i, v2 = v3)
{
v3 = i == vertices.Count - 1 ? simplified[0] : vertices[i];
float old1;
MathUtils.Cross(ref v1, ref v2, out old1);
float old2;
MathUtils.Cross(ref v2, ref v3, out old2);
float new1;
MathUtils.Cross(ref v1, ref v3, out new1);
if (Math.Abs(new1 - (old1 + old2)) > areaTolerance)
{
simplified.Add(v2);
v1 = v2;
}
}
return simplified;
}
19
Source : Joint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal void Validate(float invDt)
{
if (!Enabled)
return;
float jointErrorSquared = GetReactionForce(invDt).LengthSquared();
if (Math.Abs(jointErrorSquared) <= _breakpointSquared)
return;
Enabled = false;
if (Broke != null)
Broke(this, (float)Math.Sqrt(jointErrorSquared));
}
19
Source : PrismaticJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMreplacedA = BodyA._invMreplaced;
_invMreplacedB = BodyB._invMreplaced;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective mreplacedes.
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = (cB - cA) + rB - rA;
float mA = _invMreplacedA, mB = _invMreplacedB;
float iA = _invIA, iB = _invIB;
// Compute motor Jacobian and effective mreplaced.
{
_axis = MathUtils.Mul(qA, LocalXAxis);
_a1 = MathUtils.Cross(d + rA, _axis);
_a2 = MathUtils.Cross(rB, _axis);
_motorMreplaced = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMreplaced > 0.0f)
{
_motorMreplaced = 1.0f / _motorMreplaced;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Mul(qA, _localYAxisA);
_s1 = MathUtils.Cross(d + rA, _perp);
_s2 = MathUtils.Cross(rB, _perp);
float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
float k12 = iA * _s1 + iB * _s2;
float k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * _a1 + iB * _a2;
float k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
_K.ex = new Vector3(k11, k12, k13);
_K.ey = new Vector3(k12, k22, k23);
_K.ez = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
if (_enableMotor == false)
{
MotorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
MotorImpulse *= data.step.dtRatio;
Vector2 P = _impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis;
float LA = _impulse.X * _s1 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a1;
float LB = _impulse.X * _s2 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
_impulse = Vector3.Zero;
MotorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
19
Source : EarclipDecomposer.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
private static List<Vertices> TriangulatePolygon(Vertices vertices, float tolerance)
{
//FPE note: Check is needed as invalid triangles can be returned in recursive calls.
if (vertices.Count < 3)
return new List<Vertices>();
List<Vertices> results = new List<Vertices>();
//Recurse and split on pinch points
Vertices pA, pB;
Vertices pin = new Vertices(vertices);
if (ResolvePinchPoint(pin, out pA, out pB, tolerance))
{
List<Vertices> mergeA = TriangulatePolygon(pA, tolerance);
List<Vertices> mergeB = TriangulatePolygon(pB, tolerance);
if (mergeA.Count == -1 || mergeB.Count == -1)
throw new Exception("Can't triangulate your polygon.");
for (int i = 0; i < mergeA.Count; ++i)
{
results.Add(new Vertices(mergeA[i]));
}
for (int i = 0; i < mergeB.Count; ++i)
{
results.Add(new Vertices(mergeB[i]));
}
return results;
}
Vertices[] buffer = new Vertices[vertices.Count - 2];
int bufferSize = 0;
float[] xrem = new float[vertices.Count];
float[] yrem = new float[vertices.Count];
for (int i = 0; i < vertices.Count; ++i)
{
xrem[i] = vertices[i].X;
yrem[i] = vertices[i].Y;
}
int vNum = vertices.Count;
while (vNum > 3)
{
// Find an ear
int earIndex = -1;
float earMaxMinCross = -10.0f;
for (int i = 0; i < vNum; ++i)
{
if (IsEar(i, xrem, yrem, vNum))
{
int lower = Remainder(i - 1, vNum);
int upper = Remainder(i + 1, vNum);
Vector2 d1 = new Vector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]);
Vector2 d2 = new Vector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]);
Vector2 d3 = new Vector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]);
d1.Normalize();
d2.Normalize();
d3.Normalize();
float cross12;
MathUtils.Cross(ref d1, ref d2, out cross12);
cross12 = Math.Abs(cross12);
float cross23;
MathUtils.Cross(ref d2, ref d3, out cross23);
cross23 = Math.Abs(cross23);
float cross31;
MathUtils.Cross(ref d3, ref d1, out cross31);
cross31 = Math.Abs(cross31);
//Find the maximum minimum angle
float minCross = Math.Min(cross12, Math.Min(cross23, cross31));
if (minCross > earMaxMinCross)
{
earIndex = i;
earMaxMinCross = minCross;
}
}
}
// If we still haven't found an ear, we're screwed.
// Note: sometimes this is happening because the
// remaining points are collinear. Really these
// should just be thrown out without halting triangulation.
if (earIndex == -1)
{
for (int i = 0; i < bufferSize; i++)
{
results.Add(buffer[i]);
}
return results;
}
// Clip off the ear:
// - remove the ear tip from the list
--vNum;
float[] newx = new float[vNum];
float[] newy = new float[vNum];
int currDest = 0;
for (int i = 0; i < vNum; ++i)
{
if (currDest == earIndex) ++currDest;
newx[i] = xrem[currDest];
newy[i] = yrem[currDest];
++currDest;
}
// - add the clipped triangle to the triangle list
int under = (earIndex == 0) ? (vNum) : (earIndex - 1);
int over = (earIndex == vNum) ? 0 : (earIndex + 1);
Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under],
yrem[under]);
buffer[bufferSize] = toAdd;
++bufferSize;
// - replace the old list with the new one
xrem = newx;
yrem = newy;
}
Triangle tooAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2], xrem[0], yrem[0]);
buffer[bufferSize] = tooAdd;
++bufferSize;
for (int i = 0; i < bufferSize; i++)
{
results.Add(new Vertices(buffer[i]));
}
return results;
}
19
Source : LineTools.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public static bool LineIntersect2(ref Vector2 a0, ref Vector2 a1, ref Vector2 b0, ref Vector2 b1, out Vector2 intersectionPoint)
{
intersectionPoint = Vector2.Zero;
if (a0 == b0 || a0 == b1 || a1 == b0 || a1 == b1)
return false;
float x1 = a0.X;
float y1 = a0.Y;
float x2 = a1.X;
float y2 = a1.Y;
float x3 = b0.X;
float y3 = b0.Y;
float x4 = b1.X;
float y4 = b1.Y;
//AABB early exit
if (Math.Max(x1, x2) < Math.Min(x3, x4) || Math.Max(x3, x4) < Math.Min(x1, x2))
return false;
if (Math.Max(y1, y2) < Math.Min(y3, y4) || Math.Max(y3, y4) < Math.Min(y1, y2))
return false;
float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3));
float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3));
float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
if (Math.Abs(denom) < Settings.Epsilon)
{
//Lines are too close to parallel to call
return false;
}
ua /= denom;
ub /= denom;
if ((0 < ua) && (ua < 1) && (0 < ub) && (ub < 1))
{
intersectionPoint.X = (x1 + ua * (x2 - x1));
intersectionPoint.Y = (y1 + ua * (y2 - y1));
return true;
}
return false;
}
19
Source : MathHelper.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public static float Distance(float value1, float value2)
{
return Math.Abs(value1 - value2);
}
19
Source : AngleJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override void SolveVelocityConstraints(ref SolverData data)
{
int indexA = BodyA.IslandIndex;
int indexB = BodyB.IslandIndex;
float p = (_bias - data.velocities[indexB].w + data.velocities[indexA].w) * _mreplacedFactor;
data.velocities[indexA].w -= BodyA._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
data.velocities[indexB].w += BodyB._invI * Math.Sign(p) * Math.Min(Math.Abs(p), MaxImpulse);
}
19
Source : DistanceJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override bool SolvePositionConstraints(ref SolverData data)
{
if (Frequency > 0.0f)
{
// There is no position correction for soft distance constraints.
return true;
}
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 u = cB + rB - cA - rA;
float length = u.Length(); u.Normalize();
float C = length - Length;
C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float impulse = -_mreplaced * C;
Vector2 P = impulse * u;
cA -= _invMreplacedA * P;
aA -= _invIA * MathUtils.Cross(rA, P);
cB += _invMreplacedB * P;
aB += _invIB * MathUtils.Cross(rB, P);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return Math.Abs(C) < Settings.LinearSlop;
}
19
Source : PulleyJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// Get the pulley axes.
Vector2 uA = cA + rA - WorldAnchorA;
Vector2 uB = cB + rB - WorldAnchorB;
float lengthA = uA.Length();
float lengthB = uB.Length();
if (lengthA > 10.0f * Settings.LinearSlop)
{
uA *= 1.0f / lengthA;
}
else
{
uA = Vector2.Zero;
}
if (lengthB > 10.0f * Settings.LinearSlop)
{
uB *= 1.0f / lengthB;
}
else
{
uB = Vector2.Zero;
}
// Compute effective mreplaced.
float ruA = MathUtils.Cross(rA, uA);
float ruB = MathUtils.Cross(rB, uB);
float mA = _invMreplacedA + _invIA * ruA * ruA;
float mB = _invMreplacedB + _invIB * ruB * ruB;
float mreplaced = mA + Ratio * Ratio * mB;
if (mreplaced > 0.0f)
{
mreplaced = 1.0f / mreplaced;
}
float C = Constant - lengthA - Ratio * lengthB;
float linearError = Math.Abs(C);
float impulse = -mreplaced * C;
Vector2 PA = -impulse * uA;
Vector2 PB = -Ratio * impulse * uB;
cA += _invMreplacedA * PA;
aA += _invIA * MathUtils.Cross(rA, PA);
cB += _invMreplacedB * PB;
aB += _invIB * MathUtils.Cross(rB, PB);
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return linearError < Settings.LinearSlop;
}
19
Source : Quaternion.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float GetAngleFromQuaternion(in Quaternion q)
{
float qw = Math.Abs(q.W);
if (qw > 1)
return 0;
return 2 * (float)Math.Acos(qw);
}
19
Source : SimplifyTools.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public static Vertices MergeParallelEdges(Vertices vertices, float tolerance)
{
//From Eric Jordan's convex decomposition library
if (vertices.Count <= 3)
return vertices; //Can't do anything useful here to a triangle
bool[] mergeMe = new bool[vertices.Count];
int newNVertices = vertices.Count;
//Gather points to process
for (int i = 0; i < vertices.Count; ++i)
{
int lower = (i == 0) ? (vertices.Count - 1) : (i - 1);
int middle = i;
int upper = (i == vertices.Count - 1) ? (0) : (i + 1);
float dx0 = vertices[middle].X - vertices[lower].X;
float dy0 = vertices[middle].Y - vertices[lower].Y;
float dx1 = vertices[upper].Y - vertices[middle].X;
float dy1 = vertices[upper].Y - vertices[middle].Y;
float norm0 = (float)Math.Sqrt(dx0 * dx0 + dy0 * dy0);
float norm1 = (float)Math.Sqrt(dx1 * dx1 + dy1 * dy1);
if (!(norm0 > 0.0f && norm1 > 0.0f) && newNVertices > 3)
{
//Merge identical points
mergeMe[i] = true;
--newNVertices;
}
dx0 /= norm0;
dy0 /= norm0;
dx1 /= norm1;
dy1 /= norm1;
float cross = dx0 * dy1 - dx1 * dy0;
float dot = dx0 * dx1 + dy0 * dy1;
if (Math.Abs(cross) < tolerance && dot > 0 && newNVertices > 3)
{
mergeMe[i] = true;
--newNVertices;
}
else
mergeMe[i] = false;
}
if (newNVertices == vertices.Count || newNVertices == 0)
return vertices;
int currIndex = 0;
//Copy the vertices to a new list and clear the old
Vertices newVertices = new Vertices(newNVertices);
for (int i = 0; i < vertices.Count; ++i)
{
if (mergeMe[i] || newNVertices == 0 || currIndex == newNVertices)
continue;
Debug.replacedert(currIndex < newNVertices);
newVertices.Add(vertices[i]);
++currIndex;
}
return newVertices;
}
19
Source : VelocityLimitController.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public override void Update(float dt)
{
foreach (Body body in _bodies)
{
if (!IsActiveOn(body))
continue;
if (LimitLinearVelocity)
{
//Translation
// Check for large velocities.
float translationX = dt * body._linearVelocity.X;
float translationY = dt * body._linearVelocity.Y;
float result = translationX * translationX + translationY * translationY;
if (result > dt * _maxLinearSqared)
{
float sq = (float)Math.Sqrt(result);
float ratio = _maxLinearVelocity / sq;
body._linearVelocity.X *= ratio;
body._linearVelocity.Y *= ratio;
}
}
if (LimitAngularVelocity)
{
//Rotation
float rotation = dt * body._angularVelocity;
if (rotation * rotation > _maxAngularSqared)
{
float ratio = _maxAngularVelocity / Math.Abs(rotation);
body._angularVelocity *= ratio;
}
}
}
}
19
Source : PrismaticJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMreplacedA, mB = _invMreplacedB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (_enableLimit)
{
float translation = Vector2.Dot(axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * perp + impulse.Z * axis;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
19
Source : RevoluteJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float angularError = 0.0f;
float positionError;
bool fixedRotation = (_invIA + _invIB == 0.0f);
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
float angle = aB - aA - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMreplaced * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMreplaced * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMreplaced * C;
}
aA -= _invIA * limitImpulse;
aB += _invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 C = cB + rB - cA - rA;
positionError = C.Length();
float mA = _invMreplacedA, mB = _invMreplacedB;
float iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
Vector2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * MathUtils.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * MathUtils.Cross(rB, impulse);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
19
Source : WeldJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMreplacedA, mB = _invMreplacedB;
float iA = _invIA, iB = _invIB;
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
float positionError, angularError;
Mat33 K = new Mat33();
K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB;
K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB;
K.ez.X = -rA.Y * iA - rB.Y * iB;
K.ex.Y = K.ey.X;
K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB;
K.ez.Y = rA.X * iA + rB.X * iB;
K.ex.Z = K.ez.X;
K.ey.Z = K.ez.Y;
K.ez.Z = iA + iB;
if (FrequencyHz > 0.0f)
{
Vector2 C1 = cB + rB - cA - rA;
positionError = C1.Length();
angularError = 0.0f;
Vector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
Vector2 C1 = cB + rB - cA - rA;
float C2 = aB - aA - ReferenceAngle;
positionError = C1.Length();
angularError = Math.Abs(C2);
Vector3 C = new Vector3(C1.X, C1.Y, C2);
Vector3 impulse = -K.Solve33(C);
Vector2 P = new Vector2(impulse.X, impulse.Y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.Z);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
19
Source : WheelJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = (cB - cA) + rB - rA;
Vector2 ay = MathUtils.Mul(qA, _localYAxis);
float sAy = MathUtils.Cross(d + rA, ay);
float sBy = MathUtils.Cross(rB, ay);
float C = Vector2.Dot(d, ay);
float k = _invMreplacedA + _invMreplacedB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy;
float impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
Vector2 P = impulse * ay;
float LA = impulse * sAy;
float LB = impulse * sBy;
cA -= _invMreplacedA * P;
aA -= _invIA * LA;
cB += _invMreplacedB * P;
aB += _invIB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return Math.Abs(C) <= Settings.LinearSlop;
}
19
Source : Island.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
Vector2 c = b._sweep.C;
float a = b._sweep.A;
Vector2 v = b._linearVelocity;
float w = b._angularVelocity;
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
if (b.BodyType == BodyType.Dynamic)
{
// Integrate velocities.
// FPE: Only apply gravity if the body wants it.
if (b.IgnoreGravity)
v += h * (b._invMreplaced * b._force);
else
v += h * (b.GravityScale * gravity + b._invMreplaced * b._force);
w += h * b._invI * b._torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
}
// Solver data
SolverData solverData = new SolverData();
solverData.step = step;
solverData.positions = _positions;
solverData.velocities = _velocities;
_contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
if (Settings.EnableDiagnostics)
_watch.Start();
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
_joints[i].InitVelocityConstraints(ref solverData);
}
if (Settings.EnableDiagnostics)
_watch.Stop();
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
continue;
if (Settings.EnableDiagnostics)
_watch.Start();
joint.SolveVelocityConstraints(ref solverData);
joint.Validate(step.inv_dt);
if (Settings.EnableDiagnostics)
_watch.Stop();
}
_contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
Vector2 c = _positions[i].c;
float a = _positions[i].a;
Vector2 v = _velocities[i].v;
float w = _velocities[i].w;
// Check for large velocities
Vector2 translation = h * v;
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
float ratio = Settings.MaxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
}
// Solve position constraints
bool positionSolved = false;
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
Joint joint = _joints[j];
if (!joint.Enabled)
continue;
if (Settings.EnableDiagnostics)
_watch.Start();
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
if (Settings.EnableDiagnostics)
_watch.Stop();
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
if (Settings.EnableDiagnostics)
{
JointUpdateTime = _watch.ElapsedTicks;
_watch.Reset();
}
// Copy state buffers back to the bodies
for (int i = 0; i < BodyCount; ++i)
{
Body body = Bodies[i];
body._sweep.C = _positions[i].c;
body._sweep.A = _positions[i].a;
body._linearVelocity = _velocities[i].v;
body._angularVelocity = _velocities[i].w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
continue;
if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += h;
minSleepTime = Math.Min(minSleepTime, b._sleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
19
Source : RealExplosion.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public Dictionary<Fixture, Vector2> Activate(Vector2 pos, float radius, float maxForce)
{
AABB aabb;
aabb.LowerBound = pos + new Vector2(-radius, -radius);
aabb.UpperBound = pos + new Vector2(radius, radius);
Fixture[] shapes = new Fixture[MaxShapes];
// More than 5 shapes in an explosion could be possible, but still strange.
Fixture[] containedShapes = new Fixture[5];
bool exit = false;
int shapeCount = 0;
int containedShapeCount = 0;
// Query the world for overlapping shapes.
World.QueryAABB(
fixture =>
{
if (fixture.TestPoint(ref pos))
{
if (IgnoreWhenInsideShape)
{
exit = true;
return false;
}
containedShapes[containedShapeCount++] = fixture;
}
else
{
shapes[shapeCount++] = fixture;
}
// Continue the query.
return true;
}, ref aabb);
if (exit)
return new Dictionary<Fixture, Vector2>();
Dictionary<Fixture, Vector2> exploded = new Dictionary<Fixture, Vector2>(shapeCount + containedShapeCount);
// Per shape max/min angles for now.
float[] vals = new float[shapeCount * 2];
int valIndex = 0;
for (int i = 0; i < shapeCount; ++i)
{
PolygonShape ps;
CircleShape cs = shapes[i].Shape as CircleShape;
if (cs != null)
{
// We create a "diamond" approximation of the circle
Vertices v = new Vertices();
Vector2 vec = Vector2.Zero + new Vector2(cs.Radius, 0);
v.Add(vec);
vec = Vector2.Zero + new Vector2(0, cs.Radius);
v.Add(vec);
vec = Vector2.Zero + new Vector2(-cs.Radius, cs.Radius);
v.Add(vec);
vec = Vector2.Zero + new Vector2(0, -cs.Radius);
v.Add(vec);
ps = new PolygonShape(v, 0);
}
else
ps = shapes[i].Shape as PolygonShape;
if ((shapes[i].Body.BodyType == BodyType.Dynamic) && ps != null)
{
Vector2 toCentroid = shapes[i].Body.GetWorldPoint(ps.MreplacedData.Centroid) - pos;
float angleToCentroid = (float)Math.Atan2(toCentroid.Y, toCentroid.X);
float min = float.MaxValue;
float max = float.MinValue;
float minAbsolute = 0.0f;
float maxAbsolute = 0.0f;
for (int j = 0; j < ps.Vertices.Count; ++j)
{
Vector2 toVertex = (shapes[i].Body.GetWorldPoint(ps.Vertices[j]) - pos);
float newAngle = (float)Math.Atan2(toVertex.Y, toVertex.X);
float diff = (newAngle - angleToCentroid);
diff = (diff - MathHelper.Pi) % (2 * MathHelper.Pi);
// the minus pi is important. It means cutoff for going other direction is at 180 deg where it needs to be
if (diff < 0.0f)
diff += 2 * MathHelper.Pi; // correction for not handling negs
diff -= MathHelper.Pi;
if (Math.Abs(diff) > MathHelper.Pi)
continue; // Something's wrong, point not in shape but exists angle diff > 180
if (diff > max)
{
max = diff;
maxAbsolute = newAngle;
}
if (diff < min)
{
min = diff;
minAbsolute = newAngle;
}
}
vals[valIndex] = minAbsolute;
++valIndex;
vals[valIndex] = maxAbsolute;
++valIndex;
}
}
Array.Sort(vals, 0, valIndex, _rdc);
_data.Clear();
bool rayMissed = true;
for (int i = 0; i < valIndex; ++i)
{
Fixture fixture = null;
float midpt;
int iplus = (i == valIndex - 1 ? 0 : i + 1);
if (vals[i] == vals[iplus])
continue;
if (i == valIndex - 1)
{
// the single edgecase
midpt = (vals[0] + MathHelper.Pi * 2 + vals[i]);
}
else
{
midpt = (vals[i + 1] + vals[i]);
}
midpt = midpt / 2;
Vector2 p1 = pos;
Vector2 p2 = radius * new Vector2((float)Math.Cos(midpt), (float)Math.Sin(midpt)) + pos;
// RaycastOne
bool hitClosest = false;
World.RayCast((f, p, n, fr) =>
{
Body body = f.Body;
if (!IsActiveOn(body))
return 0;
hitClosest = true;
fixture = f;
return fr;
}, p1, p2);
//draws radius points
if ((hitClosest) && (fixture.Body.BodyType == BodyType.Dynamic))
{
if ((_data.Any()) && (_data.Last().Body == fixture.Body) && (!rayMissed))
{
int laPos = _data.Count - 1;
ShapeData la = _data[laPos];
la.Max = vals[iplus];
_data[laPos] = la;
}
else
{
// make new
ShapeData d;
d.Body = fixture.Body;
d.Min = vals[i];
d.Max = vals[iplus];
_data.Add(d);
}
if ((_data.Count > 1)
&& (i == valIndex - 1)
&& (_data.Last().Body == _data.First().Body)
&& (_data.Last().Max == _data.First().Min))
{
ShapeData fi = _data[0];
fi.Min = _data.Last().Min;
_data.RemoveAt(_data.Count - 1);
_data[0] = fi;
while (_data.First().Min >= _data.First().Max)
{
fi.Min -= MathHelper.Pi * 2;
_data[0] = fi;
}
}
int lastPos = _data.Count - 1;
ShapeData last = _data[lastPos];
while ((_data.Count > 0)
&& (_data.Last().Min >= _data.Last().Max)) // just making sure min<max
{
last.Min = _data.Last().Min - 2 * MathHelper.Pi;
_data[lastPos] = last;
}
rayMissed = false;
}
else
{
rayMissed = true; // raycast did not find a shape
}
}
for (int i = 0; i < _data.Count; ++i)
{
if (!IsActiveOn(_data[i].Body))
continue;
float arclen = _data[i].Max - _data[i].Min;
float first = MathHelper.Min(MaxEdgeOffset, EdgeRatio * arclen);
int insertedRays = (int)Math.Ceiling(((arclen - 2.0f * first) - (MinRays - 1) * MaxAngle) / MaxAngle);
if (insertedRays < 0)
insertedRays = 0;
float offset = (arclen - first * 2.0f) / ((float)MinRays + insertedRays - 1);
//Note: This loop can go into infinite as it operates on floats.
//Added FloatEquals with a large epsilon.
for (float j = _data[i].Min + first;
j < _data[i].Max || MathUtils.FloatEquals(j, _data[i].Max, 0.0001f);
j += offset)
{
Vector2 p1 = pos;
Vector2 p2 = pos + radius * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
Vector2 hitpoint = Vector2.Zero;
float minlambda = float.MaxValue;
List<Fixture> fl = _data[i].Body.FixtureList;
for (int x = 0; x < fl.Count; x++)
{
Fixture f = fl[x];
RayCastInput ri;
ri.Point1 = p1;
ri.Point2 = p2;
ri.MaxFraction = 50f;
RayCastOutput ro;
if (f.RayCast(out ro, ref ri, 0))
{
if (minlambda > ro.Fraction)
{
minlambda = ro.Fraction;
hitpoint = ro.Fraction * p2 + (1 - ro.Fraction) * p1;
}
}
// the force that is to be applied for this particular ray.
// offset is angular coverage. lambda*length of segment is distance.
float impulse = (arclen / (MinRays + insertedRays)) * maxForce * 180.0f / MathHelper.Pi * (1.0f - Math.Min(1.0f, minlambda));
// We Apply the impulse!!!
Vector2 vectImp = Vector2.Dot(impulse * new Vector2((float)Math.Cos(j), (float)Math.Sin(j)), -ro.Normal) * new Vector2((float)Math.Cos(j), (float)Math.Sin(j));
_data[i].Body.ApplyLinearImpulse(ref vectImp, ref hitpoint);
// We gather the fixtures for returning them
if (exploded.ContainsKey(f))
exploded[f] += vectImp;
else
exploded.Add(f, vectImp);
if (minlambda > 1.0f)
hitpoint = p2;
}
}
}
// We check contained shapes
for (int i = 0; i < containedShapeCount; ++i)
{
Fixture fix = containedShapes[i];
if (!IsActiveOn(fix.Body))
continue;
float impulse = MinRays * maxForce * 180.0f / MathHelper.Pi;
Vector2 hitPoint;
CircleShape circShape = fix.Shape as CircleShape;
if (circShape != null)
{
hitPoint = fix.Body.GetWorldPoint(circShape.Position);
}
else
{
PolygonShape shape = fix.Shape as PolygonShape;
hitPoint = fix.Body.GetWorldPoint(shape.MreplacedData.Centroid);
}
Vector2 vectImp = impulse * (hitPoint - pos);
fix.Body.ApplyLinearImpulse(ref vectImp, ref hitPoint);
if (!exploded.ContainsKey(fix))
exploded.Add(fix, vectImp);
}
return exploded;
}
19
Source : Math.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
public static bool FloatEquals(float value1, float value2)
{
return Math.Abs(value1 - value2) <= Settings.Epsilon;
}
19
Source : RevoluteJoint.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMreplacedA = BodyA._invMreplaced;
_invMreplacedB = BodyB._invMreplaced;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMreplacedA, mB = _invMreplacedB;
float iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
_mreplaced.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
_mreplaced.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
_mreplaced.ez.X = -_rA.Y * iA - _rB.Y * iB;
_mreplaced.ex.Y = _mreplaced.ey.X;
_mreplaced.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
_mreplaced.ez.Y = _rA.X * iA + _rB.X * iB;
_mreplaced.ex.Z = _mreplaced.ez.X;
_mreplaced.ey.Z = _mreplaced.ez.Y;
_mreplaced.ez.Z = iA + iB;
_motorMreplaced = iA + iB;
if (_motorMreplaced > 0.0f)
{
_motorMreplaced = 1.0f / _motorMreplaced;
}
if (_enableMotor == false || fixedRotation)
{
_motorImpulse = 0.0f;
}
if (_enableLimit && fixedRotation == false)
{
float jointAngle = aB - aA - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
_motorImpulse *= data.step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
19
Source : Island.cs
with MIT License
from Alan-FGR
with MIT License
from Alan-FGR
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
{
Debug.replacedert(toiIndexA < BodyCount);
Debug.replacedert(toiIndexB < BodyCount);
// Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
_positions[i].c = b._sweep.C;
_positions[i].a = b._sweep.A;
_velocities[i].v = b._linearVelocity;
_velocities[i].w = b._angularVelocity;
}
_contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting);
// Solve position constraints.
for (int i = 0; i < Settings.TOIPositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
// Leap of faith to new safe state.
Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c;
Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a;
Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c;
Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
_contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < Settings.TOIVelocityIterations; ++i)
{
_contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float h = subStep.dt;
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
Vector2 c = _positions[i].c;
float a = _positions[i].a;
Vector2 v = _velocities[i].v;
float w = _velocities[i].w;
// Check for large velocities
Vector2 translation = h * v;
if (Vector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
float ratio = Settings.MaxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
// Sync bodies
Body body = Bodies[i];
body._sweep.C = c;
body._sweep.A = a;
body._linearVelocity = v;
body._angularVelocity = w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
}
19
Source : QuadTreeCameraMovement.cs
with MIT License
from alen-smajic
with MIT License
from alen-smajic
void PanMapUsingKeyBoard(float xMove, float zMove)
{
if (Math.Abs(xMove) > 0.0f || Math.Abs(zMove) > 0.0f)
{
// Get the number of degrees in a tile at the current zoom level.
// Divide it by the tile width in pixels ( 256 in our case)
// to get degrees represented by each pixel.
// Keyboard offset is in pixels, therefore multiply the factor with the offset to move the center.
float factor = _panSpeed * (Conversions.GetTileScaleInDegrees((float)_mapManager.CenterLareplacedudeLongitude.x, _mapManager.AbsoluteZoom));
var lareplacedudeLongitude = new Vector2d(_mapManager.CenterLareplacedudeLongitude.x + zMove * factor * 2.0f, _mapManager.CenterLareplacedudeLongitude.y + xMove * factor * 4.0f);
_mapManager.UpdateMap(lareplacedudeLongitude, _mapManager.Zoom);
}
}
19
Source : AbstractMap.cs
with MIT License
from alen-smajic
with MIT License
from alen-smajic
public virtual void UpdateMap(Vector2d latLon, float zoom)
{
if (Application.isEditor && !Application.isPlaying && !IsEditorPreviewEnabled)
{
return;
}
//so map will be snapped to zero using next new tile loaded
_worldHeightFixed = false;
float differenceInZoom = 0.0f;
bool isAtInitialZoom = false;
// Update map zoom, if it has changed.
if (Math.Abs(Zoom - zoom) > Constants.EpsilonFloatingPoint)
{
SetZoom(zoom);
}
// Compute difference in zoom. Will be used to calculate correct scale of the map.
differenceInZoom = Zoom - InitialZoom;
isAtInitialZoom = (differenceInZoom - 0.0 < Constants.EpsilonFloatingPoint);
//Update center lareplacedude longitude
var centerLareplacedudeLongitude = latLon;
double xDelta = centerLareplacedudeLongitude.x;
double zDelta = centerLareplacedudeLongitude.y;
xDelta = xDelta > 0 ? Mathd.Min(xDelta, Mapbox.Utils.Constants.LareplacedudeMax) : Mathd.Max(xDelta, -Mapbox.Utils.Constants.LareplacedudeMax);
zDelta = zDelta > 0 ? Mathd.Min(zDelta, Mapbox.Utils.Constants.LongitudeMax) : Mathd.Max(zDelta, -Mapbox.Utils.Constants.LongitudeMax);
//Set Center in Lareplacedude Longitude and Mercator.
SetCenterLareplacedudeLongitude(new Vector2d(xDelta, zDelta));
Options.scalingOptions.scalingStrategy.SetUpScaling(this);
Options.placementOptions.placementStrategy.SetUpPlacement(this);
//Scale the map accordingly.
if (Math.Abs(differenceInZoom) > Constants.EpsilonFloatingPoint || isAtInitialZoom)
{
_mapScaleFactor = Vector3.one * Mathf.Pow(2, differenceInZoom);
Root.localScale = _mapScaleFactor;
}
//Update Tile extent.
if (TileProvider != null)
{
TileProvider.UpdateTileExtent();
}
if (OnUpdated != null)
{
OnUpdated();
}
}
19
Source : Earcut.cs
with MIT License
from alen-smajic
with MIT License
from alen-smajic
private static Node FindHoleBridge(Node hole, Node outerNode)
{
var p = outerNode;
var hx = hole.x;
var hy = hole.y;
var qx = float.MinValue;
Node m = null;
// find a segment intersected by a ray from the hole's leftmost point to the left;
// segment's endpoint with lesser x will be potential connection point
do
{
if (hy <= p.y && hy >= p.next.y && p.next.y != p.y)
{
var x = p.x + (hy - p.y) * (p.next.x - p.x) / (p.next.y - p.y);
if (x <= hx && x > qx)
{
qx = x;
if (x == hx)
{
if (hy == p.y) return p;
if (hy == p.next.y) return p.next;
}
m = p.x < p.next.x ? p : p.next;
}
}
p = p.next;
} while (p != outerNode);
if (m == null) return null;
if (hx == qx) return m.prev; // hole touches outer segment; pick lower endpoint
// look for points inside the triangle of hole point, segment intersection and endpoint;
// if there are no points found, we have a valid connection;
// otherwise choose the point of the minimum angle with the ray as connection point
var stop = m;
var mx = m.x;
var my = m.y;
var tanMin = float.MaxValue;
float tan = 0f;
p = m.next;
while (p != stop)
{
if (hx >= p.x && p.x >= mx && hx != p.x &&
pointInTriangle(hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, p.x, p.y))
{
tan = Math.Abs(hy - p.y) / (hx - p.x); // tangential
if ((tan < tanMin || (tan == tanMin && p.x > m.x)) && locallyInside(p, hole))
{
m = p;
tanMin = tan;
}
}
p = p.next;
}
return m;
}
19
Source : MeasuredTimePosition.cs
with MIT License
from AlFasGD
with MIT License
from AlFasGD
public MeasuredDuration DistanceFrom(MeasuredTimePosition other, TimeSignature timeSignature)
{
float thisBeat = GetAbsoluteBeat(timeSignature);
float otherBeat = other.GetAbsoluteBeat(timeSignature);
return new MeasuredDuration(Math.Abs(otherBeat - thisBeat), timeSignature);
}
19
Source : Math.cs
with Apache License 2.0
from Algoryx
with Apache License 2.0
from Algoryx
public static bool Approximately( float a, float b, float epsilon = 0.000001f )
{
return System.Math.Abs( a - b ) <= epsilon;
}
19
Source : Math.cs
with Apache License 2.0
from Algoryx
with Apache License 2.0
from Algoryx
public static bool EqualsZero( float value, float epsilon = 0.000001f )
{
return System.Math.Abs( value ) < epsilon;
}
19
Source : Vectors.cs
with MIT License
from AliTsuki
with MIT License
from AliTsuki
public Vector2 ToUVEdge()
{
float tempX = this.X;
float tempY = this.Y;
float divisor = 0f;
if(Math.Abs(tempX) >= Math.Abs(tempY))
{
divisor = tempX;
tempX /= Math.Abs(divisor);
tempY /= Math.Abs(divisor);
}
else
{
divisor = tempY;
tempX /= Math.Abs(divisor);
tempY /= Math.Abs(divisor);
}
tempX += 1f;
tempY += 1f;
tempX /= 2f;
tempY /= 2f;
return new Vector2(tempX, tempY);
}
19
Source : LocobotFPSAgentController.cs
with Apache License 2.0
from allenai
with Apache License 2.0
from allenai
public void MoveRelative(
float? moveMagnitude = null,
float x = 0f,
float z = 0f,
float noise = 0f,
bool forceAction = false
) {
if (!moveMagnitude.HasValue) {
moveMagnitude = gridSize;
} else if (moveMagnitude.Value <= 0f) {
throw new InvalidOperationException("moveMagnitude must be null or >= 0.");
}
if (!allowHorizontalMovement && Math.Abs(x) > 0) {
throw new InvalidOperationException("Controller does not support horizontal movement. Set AllowHorizontalMovement to true on the Controller.");
}
var moveLocal = new Vector3(x, 0, z);
float xzMag = moveLocal.magnitude;
if (xzMag > 1e-5f) {
// rotate a small amount with every movement since robot doesn't always move perfectly straight
if (this.applyActionNoise) {
var rotateNoise = (float)systemRandom.NextGaussian(rotateGaussianMu, rotateGaussianSigma / 2.0f);
transform.rotation = transform.rotation * Quaternion.Euler(new Vector3(0.0f, rotateNoise, 0.0f));
}
var moveLocalNorm = moveLocal / xzMag;
var magnitudeWithNoise = GetMoveMagnitudeWithNoise(
moveMagnitude: xzMag * moveMagnitude.Value,
noise: noise
);
actionFinished(moveInDirection(
direction: this.transform.rotation * (moveLocalNorm * magnitudeWithNoise),
forceAction: forceAction
));
} else {
errorMessage = "either x or z must be != 0 for the MoveRelative action";
actionFinished(false);
}
}
19
Source : Curve.cs
with MIT License
from allenwp
with MIT License
from allenwp
public void ComputeTangent(int keyIndex, CurveTangent tangentInType, CurveTangent tangentOutType)
{
// See http://msdn.microsoft.com/en-us/library/microsoft.xna.framework.curvetangent.aspx
var key = _keys[keyIndex];
float p0, p, p1;
p0 = p = p1 = key.Position;
float v0, v, v1;
v0 = v = v1 = key.Value;
if ( keyIndex > 0 )
{
p0 = _keys[keyIndex - 1].Position;
v0 = _keys[keyIndex - 1].Value;
}
if (keyIndex < _keys.Count-1)
{
p1 = _keys[keyIndex + 1].Position;
v1 = _keys[keyIndex + 1].Value;
}
switch (tangentInType)
{
case CurveTangent.Flat:
key.TangentIn = 0;
break;
case CurveTangent.Linear:
key.TangentIn = v - v0;
break;
case CurveTangent.Smooth:
var pn = p1 - p0;
if (Math.Abs(pn) < float.Epsilon)
key.TangentIn = 0;
else
key.TangentIn = (v1 - v0) * ((p - p0) / pn);
break;
}
switch (tangentOutType)
{
case CurveTangent.Flat:
key.TangentOut = 0;
break;
case CurveTangent.Linear:
key.TangentOut = v1 - v;
break;
case CurveTangent.Smooth:
var pn = p1 - p0;
if (Math.Abs(pn) < float.Epsilon)
key.TangentOut = 0;
else
key.TangentOut = (v1 - v0) * ((p1 - p) / pn);
break;
}
}
19
Source : Program.cs
with MIT License
from altimesh
with MIT License
from altimesh
[MethodImpl(MethodImplOptions.AggressiveInlining), IntrinsicFunction("fabsf")]
private static float fabsf(float a)
{
return (float)Math.Abs(a);
}
19
Source : Program.cs
with MIT License
from altimesh
with MIT License
from altimesh
[MethodImpl(MethodImplOptions.AggressiveInlining), IntrinsicFunction("fabsf")]
public static float fabsf(float f)
{
return Math.Abs(f);
}
19
Source : Program.cs
with MIT License
from altimesh
with MIT License
from altimesh
[IntrinsicFunction("fabsf")]
public static float fabsf(float f)
{
return Math.Abs(f);
}
19
Source : Program.cs
with MIT License
from altimesh
with MIT License
from altimesh
static void Main(string[] args)
{
const int N = 1024 * 1024 * 32;
float[] a = new float[N];
// initialization
Random random = new Random(42);
Parallel.For(0, N, i => a[i] = (float)random.NextDouble());
// hybridizer configuration
cudaDeviceProp prop;
cuda.GetDeviceProperties(out prop, 0);
int gridDimX = 16 * prop.multiProcessorCount;
int blockDimX = 256;
cuda.DeviceSetCacheConfig(cudaFuncCache.cudaFuncCachePreferShared);
HybRunner runner = HybRunner.Cuda().SetDistrib(gridDimX, 1, blockDimX, 1, 1, blockDimX * sizeof(float));
float[] buffMax = new float[1];
float[] buffAdd = new float[1];
var maxReductor = new GridReductor<MaxReductor>();
var addReductor = new GridReductor<AddReductor>();
dynamic wrapped = runner.Wrap(new EntryPoints());
// device reduction
wrapped.ReduceMax(maxReductor, buffMax, a, N);
wrapped.ReduceAdd(addReductor, buffAdd, a, N);
cuda.ERROR_CHECK(cuda.DeviceSynchronize());
// check results
float expectedMax = a.AsParallel().Aggregate((x, y) => Math.Max(x, y));
float expectedAdd = a.AsParallel().Aggregate((x, y) => x + y);
bool hasError = false;
if (buffMax[0] != expectedMax)
{
Console.Error.WriteLine($"MAX Error : {buffMax[0]} != {expectedMax}");
hasError = true;
}
// addition is not replacedociative, so results cannot be exactly the same
// https://en.wikipedia.org/wiki/replacedociative_property#Nonreplacedociativity_of_floating_point_calculation
if (Math.Abs(buffAdd[0] - expectedAdd) / expectedAdd > 1.0E-5F)
{
Console.Error.WriteLine($"ADD Error : {buffAdd[0]} != {expectedAdd}");
hasError = true;
}
if (hasError)
Environment.Exit(1);
Console.Out.WriteLine("OK");
}
19
Source : Program.cs
with MIT License
from altimesh
with MIT License
from altimesh
static void Main(string[] args)
{
const int N = 1024 * 1024 * 32;
float[] a = new float[N];
// initialization
Random random = new Random(42);
Parallel.For(0, N, i => a[i] = (float)random.NextDouble());
// hybridizer configuration
cudaDeviceProp prop;
cuda.GetDeviceProperties(out prop, 0);
int gridDimX = 8 * prop.multiProcessorCount;
int blockDimX = 128;
cuda.DeviceSetCacheConfig(cudaFuncCache.cudaFuncCachePreferShared);
HybRunner runner = HybRunner.Cuda().SetDistrib(gridDimX, 1, blockDimX, 1, 1, blockDimX * sizeof(float));
float[] buffMax = new float[1];
float[] buffAdd = new float[1];
dynamic wrapped = runner.Wrap(new Program());
// device reduction
cuda.ERROR_CHECK((cudaError_t) wrapped.Reduce(buffMax, a, N, new MaxLocalReductor()));
cuda.ERROR_CHECK((cudaError_t) wrapped.Reduce(buffAdd, a, N, new AddLocalReductor()));
// check results
float expectedMax = a.AsParallel().Aggregate((x, y) => Math.Max(x, y));
float expectedAdd = a.AsParallel().Aggregate((x, y) => x + y);
bool hasError = false;
if (buffMax[0] != expectedMax)
{
Console.Error.WriteLine($"MAX Error : {buffMax[0]} != {expectedMax}");
hasError = true;
}
// addition is not replacedociative, so results cannot be exactly the same
// https://en.wikipedia.org/wiki/replacedociative_property#Nonreplacedociativity_of_floating_point_calculation
if (Math.Abs(buffAdd[0] - expectedAdd) / expectedAdd > 1.0E-5F)
{
Console.Error.WriteLine($"ADD Error : {buffAdd[0]} != {expectedAdd}");
hasError = true;
}
if (hasError)
Environment.Exit(1);
Console.Out.WriteLine("OK");
}
19
Source : Program.cs
with MIT License
from altimesh
with MIT License
from altimesh
static void Main(string[] args)
{
const int N = 1024 * 1024 * 32;
float[] a = new float[N];
// initialization
Random random = new Random(42);
Parallel.For(0, N, i => a[i] = (float) random.NextDouble());
// hybridizer configuration
cudaDeviceProp prop;
cuda.GetDeviceProperties(out prop, 0);
int gridDimX = 16 * prop.multiProcessorCount;
int blockDimX = 256;
cuda.DeviceSetCacheConfig(cudaFuncCache.cudaFuncCachePreferShared);
HybRunner runner = HybRunner.Cuda().SetDistrib(gridDimX, 1, blockDimX, 1, 1, blockDimX * sizeof(float));
float[] buffMax = new float[1];
float[] buffAdd = new float[1];
dynamic wrapped = runner.Wrap(new Program());
// device reduction
wrapped.ReduceMax(buffMax, a, N);
wrapped.ReduceAdd(buffAdd, a, N);
cuda.ERROR_CHECK(cuda.DeviceSynchronize());
// check results
float expectedMax = a.AsParallel().Aggregate((x, y) => Math.Max(x, y));
float expectedAdd = a.AsParallel().Aggregate((x, y) => x+y);
bool hasError = false;
if(buffMax[0] != expectedMax)
{
Console.Error.WriteLine($"MAX Error : {buffMax[0]} != {expectedMax}");
hasError = true;
}
// addition is not replacedociative, so results cannot be exactly the same
// https://en.wikipedia.org/wiki/replacedociative_property#Nonreplacedociativity_of_floating_point_calculation
if (Math.Abs(buffAdd[0] - expectedAdd) / expectedAdd > 1.0E-5F)
{
Console.Error.WriteLine($"ADD Error : {buffAdd[0]} != {expectedAdd}");
hasError = true;
}
if (hasError)
Environment.Exit(1);
Console.Out.WriteLine("OK");
}
19
Source : NaiveMatrix.cs
with MIT License
from altimesh
with MIT License
from altimesh
override public Boolean Equals(Object o)
{
if (o == this)
return true;
if (o.GetType() != typeof(NaiveMatrix))
return false;
NaiveMatrix m = (NaiveMatrix)o;
if (this.Height != m.Height || this.Width != m.Width)
return false;
for (int i = 0; i < this.Height; ++i)
{
for (int j = 0; j < this.Width; ++j)
{
if (Math.Abs(this[i * this.Width + j] - m[i * m.Width + j]) > 1.0E-3)
return false;
}
}
return true;
}
19
Source : Program.cs
with MIT License
from altimesh
with MIT License
from altimesh
static void Main(string[] args)
{
float[] callResult_net = new float[OPT_N];
float[] putResult_net = new float[OPT_N];
float[] stockPrice_net = new float[OPT_N];
float[] optionStrike_net = new float[OPT_N];
float[] optionYears_net = new float[OPT_N];
float[] callResult_cuda = new float[OPT_N];
float[] putResult_cuda = new float[OPT_N];
Random rand = new Random(Guid.NewGuid().GetHashCode());
for (int i = 0; i < OPT_N; ++i)
{
callResult_net[i] = 0.0f;
callResult_cuda[i] = 0.0f;
putResult_net[i] = -1.0f;
putResult_cuda[i] = -1.0f;
stockPrice_net[i] = (float)rand.NextDouble() * 25.0f + 5.0f;
optionStrike_net[i] = (float)rand.NextDouble() * 99.0f + 1.0f;
optionYears_net[i] = (float)rand.NextDouble() * 9.75f + 0.25f;
}
cudaDeviceProp prop;
cuda.GetDeviceProperties(out prop, 0);
HybRunner runner = HybRunner.Cuda().SetDistrib(8 * prop.multiProcessorCount, 256);
dynamic wrapper = runner.Wrap(new Program());
for (int i = 0; i < NUM_ITERATIONS; ++i)
{
wrapper.BlackScholes(callResult_cuda,
putResult_cuda,
stockPrice_net,
optionStrike_net,
optionYears_net,
0, OPT_N);
}
Parallel.For(0, OPT_N, (opt) =>
{
BlackScholes(callResult_net,
putResult_net,
stockPrice_net,
optionStrike_net,
optionYears_net,
opt,
opt + 1);
});
float maxCallError = 0.0F;
float maxPutError = 0.0F;
float callL2Error = 0.0F;
float putL2Error = 0.0F;
float callL1Error = 0.0F;
float putL1Error = 0.0F;
for (int i = 0; i < OPT_N; ++i)
{
float callError = Math.Abs(callResult_net[i] - callResult_cuda[i]);
float putError = Math.Abs(putResult_net[i] - putResult_cuda[i]);
callL2Error += callError * callError;
putL2Error += putError * putError;
callL1Error += callError;
putL1Error += putError;
maxCallError = maxCallError > callError ? maxCallError : callError;
maxPutError = maxPutError > putError ? maxPutError : putError;
}
callL1Error /= (float)OPT_N;
putL1Error /= (float)OPT_N;
callL2Error = Sqrtf(callL2Error) / (float)OPT_N;
putL2Error = Sqrtf(putL2Error) / (float)OPT_N;
Console.WriteLine("CALL ERRORS : Linf : {0:G17}, L2 : {1:G17}, L1: {2:G17}", maxCallError, callL2Error, callL1Error);
Console.WriteLine("PUT ERRORS : Linf : {0:G17}, L2 : {1:G17}, L1: {2:G17}", maxPutError, putL2Error, putL1Error);
}
19
Source : Program.cs
with MIT License
from altimesh
with MIT License
from altimesh
static void Main(string[] args)
{
float[] callResult_net = new float[OPT_N];
float[] putResult_net = new float[OPT_N];
float[] stockPrice_net = new float[OPT_N];
float[] optionStrike_net = new float[OPT_N];
float[] optionYears_net = new float[OPT_N];
float[] callResult_cuda = new float[OPT_N];
float[] putResult_cuda = new float[OPT_N];
Random rand = new Random(Guid.NewGuid().GetHashCode());
for (int i = 0; i < OPT_N; ++i)
{
callResult_net[i] = 0.0f;
callResult_cuda[i] = 0.0f;
putResult_net[i] = -1.0f;
putResult_cuda[i] = -1.0f;
stockPrice_net[i] = (float)rand.NextDouble() * 25.0f + 5.0f;
optionStrike_net[i] = (float)rand.NextDouble() * 99.0f + 1.0f;
optionYears_net[i] = (float)rand.NextDouble() * 9.75f + 0.25f;
}
cudaDeviceProp prop;
cuda.GetDeviceProperties(out prop, 0);
HybRunner runner = HybRunner.Cuda("BlackScholes_CUDA.dll").SetDistrib(8 * prop.multiProcessorCount, 256);
dynamic wrapper = runner.Wrap(new Program());
for (int i = 0; i < NUM_ITERATIONS; ++i)
{
wrapper.BlackScholes(callResult_cuda,
putResult_cuda,
stockPrice_net,
optionStrike_net,
optionYears_net,
0, OPT_N);
}
for (int i = 0; i < NUM_ITERATIONS; ++i)
{
Parallel.For(0, OPT_N, (opt) =>
{
BlackScholes(callResult_net,
putResult_net,
stockPrice_net,
optionStrike_net,
optionYears_net,
opt,
opt + 1);
});
}
float maxCallError = 0.0F;
float maxPutError = 0.0F;
float callL2Error = 0.0F;
float putL2Error = 0.0F;
float callL1Error = 0.0F;
float putL1Error = 0.0F;
for (int i = 0; i < OPT_N; ++i)
{
float callError = Math.Abs(callResult_net[i] - callResult_cuda[i]);
float putError = Math.Abs(putResult_net[i] - putResult_cuda[i]);
callL2Error += callError * callError;
putL2Error += putError * putError;
callL1Error += callError;
putL1Error += putError;
maxCallError = maxCallError > callError ? maxCallError : callError;
maxPutError = maxPutError > putError ? maxPutError : putError;
}
callL1Error /= (float)OPT_N;
putL1Error /= (float)OPT_N;
callL2Error = Sqrtf(callL2Error) / (float)OPT_N;
putL2Error = Sqrtf(putL2Error) / (float)OPT_N;
Console.WriteLine("CALL ERRORS : Linf : {0:G17}, L2 : {1:G17}, L1: {2:G17}", maxCallError, callL2Error, callL1Error);
Console.WriteLine("PUT ERRORS : Linf : {0:G17}, L2 : {1:G17}, L1: {2:G17}", maxPutError, putL2Error, putL1Error);
}
19
Source : 01-gpu.cs
with MIT License
from altimesh
with MIT License
from altimesh
[EntryPoint]
public static void Sobel(float[] input, float[] output, int width, int height)
{
for (int i = threadIdx.y + blockIdx.y * blockDim.y; i < height; i += blockDim.y * gridDim.y)
{
for (int j = threadIdx.x + blockIdx.x * blockDim.x; j < width; j += blockDim.x * gridDim.x)
{
int pixelId = i * width + j;
if (i != 0 && j != 0 && i != height - 1 && j != width - 1)
{
float tl = input[pixelId - width - 1];
float t = input[pixelId - width];
float tr = input[pixelId - width + 1];
float l = input[pixelId - 1];
float r = input[pixelId + 1];
float br = input[pixelId + width + 1];
float bl = input[pixelId + width - 1];
float b = input[pixelId + width];
float data = (ushort)(Math.Abs((tl + 2.0F * l + bl - tr - 2.0F * r - br)) +
Math.Abs((tl + 2.0F * t + tr - bl - 2.0F * b - br)));
if (data > 255)
{
data = 255;
}
output[pixelId] = data;
}
else
{
output[pixelId] = 0;
}
}
}
}
19
Source : 02-texture.cs
with MIT License
from altimesh
with MIT License
from altimesh
[EntryPoint]
public static void Sobel(cudaTextureObject_t texObj, float[] output, int width, int height)
{
for (int i = threadIdx.y + blockIdx.y * blockDim.y; i < height; i += blockDim.y * gridDim.y)
{
for (int j = threadIdx.x + blockIdx.x * blockDim.x; j < width; j += blockDim.x * gridDim.x)
{
int pixelId = i * width + j;
if (i != 0 && j != 0 && i != height - 1 && j != width - 1)
{
float tl = TextureHelpers.tex2D(texObj, j - 1.0F, i - 1.0F);
float t = TextureHelpers.tex2D(texObj, j - 1.0F, i);
float tr = TextureHelpers.tex2D(texObj, j - 1.0F, i + 1.0F);
float l = TextureHelpers.tex2D(texObj, j, i - 1.0F);
float r = TextureHelpers.tex2D(texObj, j, ii + 1.0F);
float br = TextureHelpers.tex2D(texObj, j + 1.0F, i + 1.0F);
float bl = TextureHelpers.tex2D(texObj, j + 1.0F, i - 1.0F);
float b = TextureHelpers.tex2D(texObj, j + 1.0F, i);
float data = (Math.Abs((tl + 2.0F * l + bl - tr - 2.0F * r - br)) +
Math.Abs((tl + 2.0F * t + tr - bl - 2.0F * b - br)));
if (data > 255)
{
data = 255;
}
output[pixelId] = data;
}
else
{
output[pixelId] = 0;
}
}
}
}
See More Examples