public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, GGame.Math.Fix64 amount) { return(new Vector3( MathHelper.SmoothStep(value1.X, value2.X, amount), MathHelper.SmoothStep(value1.Y, value2.Y, amount), MathHelper.SmoothStep(value1.Z, value2.Z, amount))); }
public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result) { GGame.Math.Fix64 dot = Dot(vector, normal); result.X = vector.X - ((2f * dot) * normal.X); result.Y = vector.Y - ((2f * dot) * normal.Y); result.Z = vector.Z - ((2f * dot) * normal.Z); }
public static Vector3 Multiply(Vector3 value1, GGame.Math.Fix64 scaleFactor) { value1.X *= scaleFactor; value1.Y *= scaleFactor; value1.Z *= scaleFactor; return(value1); }
public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, GGame.Math.Fix64 amount, out Vector3 result) { result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount); result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount); result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount); }
public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, GGame.Math.Fix64 amount, out Vector3 result) { result = new Vector3( MathHelper.SmoothStep(value1.X, value2.X, amount), MathHelper.SmoothStep(value1.Y, value2.Y, amount), MathHelper.SmoothStep(value1.Z, value2.Z, amount)); }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.Velocities[_indexA].V; GGame.Math.Fix64 wA = data.Velocities[_indexA].W; // Cdot = v + cross(w, r) Vector2 Cdot = vA + MathUtils.Cross(wA, _rA); Vector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse)); Vector2 oldImpulse = _impulse; _impulse += impulse; GGame.Math.Fix64 maxImpulse = data.Step.dt * MaxForce; if (_impulse.LengthSquared() > maxImpulse * maxImpulse) { _impulse *= maxImpulse / _impulse.Length(); } impulse = _impulse - oldImpulse; vA += _invMassA * impulse; wA += _invIA * MathUtils.Cross(_rA, impulse); data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; }
public static void ComputeCircleAABB(ref Vector2 pos, GGame.Math.Fix64 radius, ref Transform transform, out AABB aabb) { Vector2 p = transform.p + MathUtils.Mul(transform.q, pos); aabb.LowerBound = new Vector2(p.X - radius, p.Y - radius); aabb.UpperBound = new Vector2(p.X + radius, p.Y + radius); }
public Point(GGame.Math.Fix64 x, GGame.Math.Fix64 y) { X = x; Y = y; Next = null; Prev = null; }
public static Fixture AttachLineArc(GGame.Math.Fix64 radians, int sides, GGame.Math.Fix64 radius, bool closed, Body body) { Vertices arc = PolygonUtils.CreateArc(radians, sides, radius); arc.Rotate((MathHelper.Pi - radians) / 2); return(closed ? AttachLoopShape(arc, body) : AttachChainShape(arc, body)); }
private void ComputeMass() { //Velcro: We calculate area for later consumption GGame.Math.Fix64 area = Settings.Pi * _2radius; MassData.Area = area; MassData.Mass = Density * area; }
internal Vector2 GetSearchDirection() { switch (Count) { case 1: return(-V[0].W); case 2: { Vector2 e12 = V[1].W - V[0].W; GGame.Math.Fix64 sgn = MathUtils.Cross(e12, -V[0].W); if (sgn > 0.0f) { // Origin is left of e12. return(MathUtils.Cross(1.0f, e12)); } else { // Origin is right of e12. return(MathUtils.Cross(e12, 1.0f)); } } default: Debug.Assert(false); return(Vector2.Zero); } }
/// <summary> /// Creates a new terrain /// </summary> /// <param name="world">The World</param> /// <param name="position">The position (center) of the terrain.</param> /// <param name="width">The width of the terrain.</param> /// <param name="height">The height of the terrain.</param> public Terrain(World world, Vector2 position, GGame.Math.Fix64 width, GGame.Math.Fix64 height) { World = world; Width = width; Height = height; Center = position; }
/// <summary> /// Compute the collision manifold between two circles. /// </summary> public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB) { manifold.PointCount = 0; Vector2 pA = MathUtils.Mul(ref xfA, circleA.Position); Vector2 pB = MathUtils.Mul(ref xfB, circleB.Position); Vector2 d = pB - pA; GGame.Math.Fix64 distSqr = Vector2.Dot(d, d); GGame.Math.Fix64 rA = circleA.Radius, rB = circleB.Radius; GGame.Math.Fix64 radius = rA + rB; if (distSqr > radius * radius) { return; } manifold.Type = ManifoldType.Circles; manifold.LocalPoint = circleA.Position; manifold.LocalNormal = Vector2.Zero; manifold.PointCount = 1; ManifoldPoint p0 = manifold.Points[0]; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; manifold.Points[0] = p0; }
private GGame.Math.Fix64 GetCurvePosition(GGame.Math.Fix64 position) { //only for position in curve CurveKey prev = keys[0]; CurveKey next; for (int i = 1; i < keys.Count; i++) { next = Keys[i]; if (next.Position >= position) { if (prev.Continuity == CurveContinuity.Step) { if (position >= 1f) { return(next.Value); } return(prev.Value); } GGame.Math.Fix64 t = (position - prev.Position) / (next.Position - prev.Position); //to have t in [0,1] GGame.Math.Fix64 ts = t * t; GGame.Math.Fix64 tss = ts * t; //After a lot of search on internet I have found all about spline function // and bezier (phi'sss ancien) but finaly use hermite curve //http://en.wikipedia.org/wiki/Cubic_Hermite_spline //P(t) = (2*t^3 - 3t^2 + 1)*P0 + (t^3 - 2t^2 + t)m0 + (-2t^3 + 3t^2)P1 + (t^3-t^2)m1 //with P0.value = prev.value , m0 = prev.tangentOut, P1= next.value, m1 = next.TangentIn return((2 * tss - 3 * ts + 1f) * prev.Value + (tss - 2 * ts + t) * prev.TangentOut + (3 * ts - 2 * tss) * next.Value + (tss - ts) * next.TangentIn); } prev = next; } return(0f); }
/// <summary> /// Resets the dynamics of this body. /// Sets torque, force and linear/angular velocity to 0 /// </summary> public void ResetDynamics() { _torque = 0; _angularVelocity = 0; _force = Vector2.Zero; _linearVelocity = Vector2.Zero; }
private AdvancingFrontNode LocateNode(GGame.Math.Fix64 x) { AdvancingFrontNode node = FindSearchNode(x); if (x < node.Value) { while ((node = node.Prev) != null) { if (x >= node.Value) { Search = node; return(node); } } } else { while ((node = node.Next) != null) { if (x < node.Value) { Search = node.Prev; return(node.Prev); } } } return(null); }
/// <summary> /// Creates a new terrain. /// </summary> /// <param name="world">The World</param> /// <param name="area">The area of the terrain.</param> public Terrain(World world, AABB area) { World = world; Width = area.Width; Height = area.Height; Center = area.Center; }
public static bool TestPointCircle(ref Vector2 pos, GGame.Math.Fix64 radius, ref Vector2 point, ref Transform transform) { Vector2 center = transform.p + MathUtils.Mul(transform.q, pos); Vector2 d = point - center; return(Vector2.Dot(d, d) <= radius * radius); }
public TrapezoidalMap() { Map = new HashSet <Trapezoid>(); _margin = 50.0f; _bCross = null; _cross = null; }
// Solve a line segment using barycentric coordinates. // // p = a1 * w1 + a2 * w2 // a1 + a2 = 1 // // The vector from the origin to the closest point on the line is // perpendicular to the line. // e12 = w2 - w1 // dot(p, e) = 0 // a1 * dot(w1, e) + a2 * dot(w2, e) = 0 // // 2-by-2 linear system // [1 1 ][a1] = [1] // [w1.e12 w2.e12][a2] = [0] // // Define // d12_1 = dot(w2, e12) // d12_2 = -dot(w1, e12) // d12 = d12_1 + d12_2 // // Solution // a1 = d12_1 / d12 // a2 = d12_2 / d12 internal void Solve2() { Vector2 w1 = V[0].W; Vector2 w2 = V[1].W; Vector2 e12 = w2 - w1; // w1 region GGame.Math.Fix64 d12_2 = -Vector2.Dot(w1, e12); if (d12_2 <= 0.0f) { // a2 <= 0, so we clamp it to 0 V.Value0.A = 1.0f; Count = 1; return; } // w2 region GGame.Math.Fix64 d12_1 = Vector2.Dot(w2, e12); if (d12_1 <= 0.0f) { // a1 <= 0, so we clamp it to 0 V.Value1.A = 1.0f; Count = 1; V.Value0 = V.Value1; return; } // Must be in e12 region. GGame.Math.Fix64 inv_d12 = 1.0f / (d12_1 + d12_2); V.Value0.A = d12_1 * inv_d12; V.Value1.A = d12_2 * inv_d12; Count = 2; }
/// <summary> /// Initialize the terrain for use. /// </summary> public void Initialize() { // find top left of terrain in world space _topLeft = new Vector2(Center.X - (Width * 0.5f), Center.Y - (-Height * 0.5f)); // convert the terrains size to a point cloud size _localWidth = Width * PointsPerUnit; _localHeight = Height * PointsPerUnit; _terrainMap = new sbyte[(int)_localWidth + 1, (int)_localHeight + 1]; for (int x = 0; x < _localWidth; x++) { for (int y = 0; y < _localHeight; y++) { _terrainMap[x, y] = 1; } } _xnum = (int)(_localWidth / CellSize); _ynum = (int)(_localHeight / CellSize); _bodyMap = new List <Body> [_xnum, _ynum]; // make sure to mark the dirty area to an infinitely small box _dirtyArea = new AABB(new Vector2(GGame.Math.Fix64.MaxValue, GGame.Math.Fix64.MaxValue), new Vector2(GGame.Math.Fix64.MinValue, GGame.Math.Fix64.MinValue)); }
/// <summary> /// Decompose the polygon into several smaller non-concave polygons. /// </summary> /// <param name="vertices">The polygon to decompose.</param> /// <param name="sheer">The sheer to use if you get bad results, try using a higher value. 默认值 0.001</param> /// <returns>A list of trapezoids</returns> public static List <Vertices> ConvexPartitionTrapezoid(Vertices vertices, GGame.Math.Fix64 sheer) { List <Point> compatList = new List <Point>(vertices.Count); foreach (Vector2 vertex in vertices) { compatList.Add(new Point(vertex.X, vertex.Y)); } Triangulator t = new Triangulator(compatList, sheer); List <Vertices> list = new List <Vertices>(); foreach (Trapezoid trapezoid in t.Trapezoids) { Vertices verts = new Vertices(); List <Point> points = trapezoid.GetVertices(); foreach (Point point in points) { verts.Add(new Vector2(point.X, point.Y)); } list.Add(verts); } return(list); }
/// <summary> /// Decompose the polygon into several smaller non-concave polygons. /// </summary> /// <param name="vertices">The polygon to decompose.</param> /// <param name="sheer">The sheer to use if you get bad results, try using a higher value 默认值 0.001.</param> /// <returns>A list of triangles</returns> public static List <Vertices> ConvexPartition(Vertices vertices, GGame.Math.Fix64 sheer) { Debug.Assert(vertices.Count > 3); List <Point> compatList = new List <Point>(vertices.Count); foreach (Vector2 vertex in vertices) { compatList.Add(new Point(vertex.X, vertex.Y)); } Triangulator t = new Triangulator(compatList, sheer); List <Vertices> list = new List <Vertices>(); foreach (List <Point> triangle in t.Triangles) { Vertices outTriangles = new Vertices(triangle.Count); foreach (Point outTriangle in triangle) { outTriangles.Add(new Vector2(outTriangle.X, outTriangle.Y)); } list.Add(outTriangles); } return(list); }
public static void Divide(ref Vector3 value1, GGame.Math.Fix64 divisor, out Vector3 result) { GGame.Math.Fix64 factor = 1 / divisor; result.X = value1.X * factor; result.Y = value1.Y * factor; result.Z = value1.Z * factor; }
/// <summary> /// Set the position of the body's origin and rotation. /// This breaks any contacts and wakes the other bodies. /// Manipulating a body's transform may cause non-physical behavior. /// </summary> /// <param name="position">The world position of the body's local origin.</param> /// <param name="rotation">The world rotation in radians.</param> public void SetTransform(ref Vector2 position, GGame.Math.Fix64 rotation) { SetTransformIgnoreContacts(ref position, rotation); //Velcro: We check for new contacts after a body has been moved. _world.ContactManager.FindNewContacts(); }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.Velocities[_indexA].V; GGame.Math.Fix64 wA = data.Velocities[_indexA].W; Vector2 vB = data.Velocities[_indexB].V; GGame.Math.Fix64 wB = data.Velocities[_indexB].W; GGame.Math.Fix64 mA = _invMassA, mB = _invMassB; GGame.Math.Fix64 iA = _invIA, iB = _invIB; if (FrequencyHz > 0.0f) { GGame.Math.Fix64 Cdot2 = wB - wA; GGame.Math.Fix64 impulse2 = -_mass.ez.Z * (Cdot2 + _bias + _gamma * _impulse.Z); _impulse.Z += impulse2; wA -= iA * impulse2; wB += iB * impulse2; Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); Vector2 impulse1 = -MathUtils.Mul22(_mass, Cdot1); _impulse.X += impulse1.X; _impulse.Y += impulse1.Y; Vector2 P = impulse1; vA -= mA * P; wA -= iA * MathUtils.Cross(_rA, P); vB += mB * P; wB += iB * MathUtils.Cross(_rB, P); } else { Vector2 Cdot1 = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA); GGame.Math.Fix64 Cdot2 = wB - wA; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = -MathUtils.Mul(_mass, Cdot); _impulse += impulse; Vector2 P = new Vector2(impulse.X, impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + impulse.Z); } data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; data.Velocities[_indexB].V = vB; data.Velocities[_indexB].W = wB; }
internal override void SolveVelocityConstraints(ref SolverData data) { Vector2 vA = data.Velocities[_indexA].V; GGame.Math.Fix64 wA = data.Velocities[_indexA].W; Vector2 vB = data.Velocities[_indexB].V; GGame.Math.Fix64 wB = data.Velocities[_indexB].W; GGame.Math.Fix64 mA = _invMassA, mB = _invMassB; GGame.Math.Fix64 iA = _invIA, iB = _invIB; GGame.Math.Fix64 h = data.Step.dt; GGame.Math.Fix64 inv_h = data.Step.inv_dt; // Solve angular friction { GGame.Math.Fix64 Cdot = wB - wA + inv_h * CorrectionFactor * _angularError; GGame.Math.Fix64 impulse = -_angularMass * Cdot; GGame.Math.Fix64 oldImpulse = _angularImpulse; GGame.Math.Fix64 maxImpulse = h * _maxTorque; _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = _angularImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve linear friction { Vector2 Cdot = vB + MathUtils.Cross(wB, _rB) - vA - MathUtils.Cross(wA, _rA) + inv_h * CorrectionFactor * _linearError; Vector2 impulse = -MathUtils.Mul(ref _linearMass, ref Cdot); Vector2 oldImpulse = _linearImpulse; _linearImpulse += impulse; GGame.Math.Fix64 maxImpulse = h * _maxForce; if (_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { _linearImpulse.Normalize(); _linearImpulse *= maxImpulse; } impulse = _linearImpulse - oldImpulse; vA -= mA * impulse; wA -= iA * MathUtils.Cross(_rA, impulse); vB += mB * impulse; wB += iB * MathUtils.Cross(_rB, impulse); } data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; data.Velocities[_indexB].V = vB; data.Velocities[_indexB].W = wB; }
/// <summary> /// Advance the sweep forward, yielding a new initial state. /// </summary> /// <param name="alpha">new initial time</param> public void Advance(GGame.Math.Fix64 alpha) { Debug.Assert(Alpha0 < 1.0f); GGame.Math.Fix64 beta = (alpha - Alpha0) / (1.0f - Alpha0); C0 += beta * (C - C0); A0 += beta * (A - A0); Alpha0 = alpha; }
/// <summary> /// Moves the given body along the defined path. /// </summary> /// <param name="path">The path.</param> /// <param name="body">The body.</param> /// <param name="time">The time.</param> /// <param name="strength">The strength.</param> /// <param name="timeStep">The time step.</param> public static void MoveBodyOnPath(Path path, Body body, GGame.Math.Fix64 time, GGame.Math.Fix64 strength, GGame.Math.Fix64 timeStep) { Vector2 destination = path.GetPosition(time); Vector2 positionDelta = body.Position - destination; Vector2 velocity = (positionDelta / timeStep) * strength; body.LinearVelocity = -velocity; }
public CurveKey(GGame.Math.Fix64 position, GGame.Math.Fix64 value, GGame.Math.Fix64 tangentIn, GGame.Math.Fix64 tangentOut, CurveContinuity continuity) { this.position = position; this.value = value; this.tangentIn = tangentIn; this.tangentOut = tangentOut; this.continuity = continuity; }