System.Math.Abs(float)

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 7

19 Source : WheelShape.cs
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

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

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

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

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

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

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

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

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

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

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

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

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

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

public static float Distance(float value1, float value2)
        {
            return Math.Abs(value1 - value2);
        }

19 Source : AngleJoint.cs
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

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

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

[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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

[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

[MethodImpl(MethodImplOptions.AggressiveInlining), IntrinsicFunction("fabsf")]
        public static float fabsf(float f)
        {
            return Math.Abs(f);
        }

19 Source : Program.cs
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

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

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

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

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

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

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

[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

[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