public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b) { TFCircleCollider A = (TFCircleCollider)a.coll; TFCircleCollider B = (TFCircleCollider)b.coll; //Calculate translational vector, which is normal FixVec2 normal = ((FixVec2)b.Position) - ((FixVec2)a.Position); Fix distSpr = normal.GetMagnitudeSquared(); Fix radius = A.radius + B.radius; //Not in contact if (distSpr >= radius * radius) { m.contactCount = 0; return; } Fix distance = FixMath.Sqrt(distSpr); m.contactCount = 1; if (distance == Fix.zero) { m.penetration = A.radius; m.normal = new FixVec2(1, 0); m.contacts[0] = (FixVec2)a.Position; } else { m.penetration = radius - distance; m.normal = normal / distance; m.contacts[0] = m.normal * A.radius + ((FixVec2)a.Position); } }
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b) { TFEdgeCollider A = (TFEdgeCollider)a.coll; TFCircleCollider B = (TFCircleCollider)b.coll; // No line segments, return out. if (A.vertices.Count < 2) { return; } // Transform circle center to Edge model space FixVec2 circleCenter = A.u.Transposed() * (b.Position - a.Position); // Iterate through all the line segments to find contact point. for (int i = 0; i < A.vertices.Count - 1; i++) { FixVec2 rayDir = (A.vertices[i + 1] - A.vertices[i]); FixVec2 centerRay = (A.vertices[i] - circleCenter); Fix k = rayDir.Dot(rayDir); Fix l = 2 * centerRay.Dot(rayDir); Fix n = centerRay.Dot(centerRay) - (B.radius * B.radius); Fix discriminant = l * l - 4 * k * n; // No intersection. if (discriminant <= Fix.zero) { continue; } discriminant = FixMath.Sqrt(discriminant); Fix t1 = (-l - discriminant) / (2 * k); Fix t2 = (-l + discriminant) / (2 * k); Fix s = FixVec2.Dot(A.normals[i], circleCenter - A.vertices[i]); if (t1 >= Fix.zero && t1 <= Fix.one) { //t1 is the intersection, and it's closer than t2. m.contactCount = 1; m.contacts[0] = (A.u * A.vertices[i] + a.Position) + (t1 * rayDir); m.normal = A.normals[i]; m.penetration = B.radius - s; return; } if (t2 >= Fix.zero && t2 <= Fix.one) { // t1 didn't insection, so we either started inside the circle // or completely past it. m.contactCount = 1; m.contacts[0] = (A.u * A.vertices[i] + a.Position) + (t2 * rayDir); m.normal = A.normals[i]; m.penetration = B.radius - s; return; } } }
public override bool Raycast(out TFRaycastHit2D hit, FixVec2 pointA, FixVec2 pointB, Fix maxFraction) { hit = default; FixVec2 center = (FixVec2)tdTransform.Position; FixVec2 s = pointA - center; Fix b = FixVec2.Dot(s, s) - radius * radius; // Solve quadratic equation. FixVec2 r = pointB - pointA; Fix c = FixVec2.Dot(s, r); Fix rr = FixVec2.Dot(r, r); Fix sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < Fix.zero || rr < Fix.Epsilon) { return(false); } // Find the point of intersection on the line with the circle. Fix a = -(c + FixMath.Sqrt(sigma)); // Is the intersection point on the segment? if (Fix.zero <= a && a <= maxFraction * rr) { a /= rr; hit.fraction = a; hit.normal = s + a * r; hit.normal.Normalize(); return(true); } return(false); }
private Vector3 GetU3DPosition() { return(new Vector3(FixMath.mm2m(position.x), FixMath.mm2m(position.y), FixMath.mm2m(position.z) )); }
protected static SteeringVelocity DoReachOrientation(Entity <Game> owner, Fix targetOrientation) { var tolerance = owner.Get <AlignTolerance>(); var steering = SteeringVelocity.Zero; var orientation = owner.Get <Orientation>().Value; var rotation = ArithmeticUtils.WrapAngleAroundZero(targetOrientation - orientation); var rotationSize = FixMath.Abs(rotation); if (rotationSize <= tolerance.Angle) { return(steering); } var maxAngularVelocity = owner.Get <MaxAngularVelocity>().Value; var angular = maxAngularVelocity * rotation / rotationSize; var decelerationAngle = tolerance.DecelerationAngle; if (rotationSize <= decelerationAngle) { angular *= rotationSize / decelerationAngle; } steering.Angular = angular; return(steering); }
public virtual void ApplyTraction() { if (kineticEnergies.horizontal.X == Fix.Zero) { return; } bool startedDir = FixMath.Sign(kineticEnergies.horizontal.X) == 1 ? true : false; if (startedDir) { kineticEnergies.horizontal._x -= cInfo.attributes.traction; if (kineticEnergies.horizontal.X < Fix.Zero) { kineticEnergies.horizontal = FixVec2.Zero; } } else { kineticEnergies.horizontal._x += cInfo.attributes.traction; if (kineticEnergies.horizontal.X > Fix.Zero) { kineticEnergies.horizontal = FixVec2.Zero; } } }
/// <summary> /// Converts a quaternion to a euler angle. /// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles /// </summary> /// <param name="quaternion">The quaternion to convert from.</param> /// <returns>An euler angle.</returns> public static FixVec3 ToEuler(FixQuaternion quaternion) { FixVec3 result; Fix t0 = 2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y); Fix t1 = Fix.one - (2 * (quaternion.y * quaternion.y + quaternion.z * quaternion.z)); result.z = FixMath.Atan2(t0, t1); Fix t2 = 2 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x); if (t2 >= Fix.one) { result.y = FixMath.PI / 2; } else if (t2 <= -Fix.one) { result.y = -(FixMath.PI / 2); } else { result.y = FixMath.Asin(t2); } Fix t3 = 2 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z); Fix t4 = Fix.one - (2 * (quaternion.x * quaternion.x + quaternion.y * quaternion.y)); result.x = FixMath.Atan2(t3, t4); return(result); }
public void initialize() { // Calculate average bounciness/restitution e = FixMath.Min(A.Bounciness, B.Bounciness); // Calculate static & dynamic friction sf = FixMath.Sqrt(A.StaticFriction * A.StaticFriction + B.StaticFriction * B.StaticFriction); df = FixMath.Sqrt(A.DynamicFriction * A.DynamicFriction + B.DynamicFriction * B.DynamicFriction); for (int i = 0; i < contactCount; i++) { //Calculate radii from COM to contact FixVec2 ra = contacts[i] -= A.info.position; FixVec2 rb = contacts[i] -= B.info.position; //? FixVec2 rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb) - A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra); // Determine if we should perform a resting collision or not // The idea is if the only thing moving this object is gravity, // then the collision should be performed without any restitution if (rv.GetMagnitudeSquared() < TFPhysics.instance.resting) { e = 0; } } }
public override SteeringVelocity DoCalculate(Entity <Game> owner, ref SteeringVelocity accumulatedSteering) { var steering = new SteeringVelocity(); if (!owner.Has <FlowField>()) { return(steering); } var flowField = owner.Get <FlowField>().Field; var flowVector = flowField.LookupFlowVector(owner.Get <Position>().Value); if (!owner.Has <TargetOrientation>()) { return(SteeringVelocity.Zero); } if (flowVector.MagnitudeSqr <= ZeroVelocity * ZeroVelocity) { return(SteeringVelocity.Zero); } var targetOrientation = FixMath.Atan2(-flowVector.X, flowVector.Y); return(DoReachOrientation(owner, targetOrientation)); }
/// <summary> /// Creates a matrix for rotating points around the Z-axis, from a center point. /// </summary> /// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param> /// <param name="centerPoint">The center point.</param> /// <returns>The rotation matrix.</returns> public static FixMatrix4x4 RotateZ(Fix64 radians, FixVector3 centerPoint) { FixMatrix4x4 result; Fix64 c = FixMath.Cos(radians); Fix64 s = FixMath.Sin(radians); Fix64 x = centerPoint.x * (1 - c) + centerPoint.y * s; Fix64 y = centerPoint.y * (1 - c) - centerPoint.x * s; // [ c s 0 0 ] // [ -s c 0 0 ] // [ 0 0 1 0 ] // [ x y 0 1 ] result.M11 = c; result.M12 = s; result.M13 = Fix64.Zero; result.M14 = Fix64.Zero; result.M21 = -s; result.M22 = c; result.M23 = Fix64.Zero; result.M24 = Fix64.Zero; result.M31 = Fix64.Zero; result.M32 = Fix64.Zero; result.M33 = Fix64.One; result.M34 = Fix64.Zero; result.M41 = Fix64.Zero; result.M42 = Fix64.Zero; result.M43 = Fix64.Zero; result.M44 = Fix64.One; return(result); }
/// Returns the fraction along the segment query the cpBB is hit. Returns INFINITY if it doesn't hit. public static Fix SegmentQuery(ref AABB aabb, ref FixVec2 a, ref FixVec2 b) { var idx = 1.0f / (b.X - a.X); var tx1 = (aabb.L == a.X ? Fix.MinValue : (aabb.L - a.X) * idx); var tx2 = (aabb.R == a.X ? Fix.MaxValue : (aabb.R - a.X) * idx); var txmin = FixMath.Min(tx1, tx2); var txmax = FixMath.Max(tx1, tx2); var idy = 1.0f / (b.Y - a.Y); var ty1 = (aabb.B == a.Y ? Fix.MinValue : (aabb.B - a.Y) * idy); var ty2 = (aabb.T == a.Y ? Fix.MaxValue : (aabb.T - a.Y) * idy); var tymin = FixMath.Min(ty1, ty2); var tymax = FixMath.Max(ty1, ty2); if (tymin <= txmax && txmin <= tymax) { var min = FixMath.Max(txmin, tymin); var max = FixMath.Min(txmax, tymax); if (0 <= max && min <= 1) { return(FixMath.Max(min, 0)); } } return(Fix.MaxValue); }
void moveVertically(ref FixVec2 deltaMovement) { var isGoingUp = deltaMovement.y > 0; var rayDistance = FixMath.Abs(deltaMovement.y) + _skinWidth; var rayDirection = isGoingUp ? FixVec2.up : -FixVec2.up; var initialRayOrigin = isGoingUp ? _raycastOrigins.topLeft : _raycastOrigins.bottomLeft; // apply our horizontal deltaMovement here so that we do our raycast from the actual position we would be in if we had moved initialRayOrigin.x += deltaMovement.x; // if we are moving up, we should ignore the layers in oneWayPlatformMask var mask = platformMask; if ((isGoingUp && !collisionState.wasGroundedLastFrame) || ignoreOneWayPlatformsThisFrame) { mask &= ~oneWayPlatformMask; } for (var i = 0; i < totalVerticalRays; i++) { FixVec2 ray = new FixVec2(initialRayOrigin.x + i * _horizontalDistanceBetweenRays, initialRayOrigin.y); DrawRay((Vector3)ray, (Vector3)(rayDirection * rayDistance), Color.red); _raycastHit = TFPhysics.Raycast(ray, rayDirection, rayDistance, mask); if (_raycastHit) { // set our new deltaMovement and recalculate the rayDistance taking it into account deltaMovement.y = _raycastHit.point.y - ray.y; rayDistance = FixMath.Abs(deltaMovement.y); // remember to remove the skinWidth from our deltaMovement if (isGoingUp) { deltaMovement.y -= _skinWidth; collisionState.above = true; } else { deltaMovement.y += _skinWidth; collisionState.below = true; } _raycastHitsThisFrame.Add(_raycastHit); // this is a hack to deal with the top of slopes. if we walk up a slope and reach the apex we can get in a situation // where our ray gets a hit that is less then skinWidth causing us to be ungrounded the next frame due to residual velocity. if (!isGoingUp && deltaMovement.y > Fix.zero) { _isGoingUpSlope = true; } // we add a small fudge factor for the float operations here. if our rayDistance is smaller // than the width + fudge bail out because we have a direct impact if (rayDistance < _skinWidth + kSkinWidthFloatFudgeFactor) { break; } } } }
/// <summary> /// Creates a matrix for rotating points around the Y-axis, from a center point. /// </summary> /// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param> /// <param name="centerPoint">The center point.</param> /// <returns>The rotation matrix.</returns> public static FixMatrix4x4 RotateY(Fix64 radians, FixVector3 centerPoint) { FixMatrix4x4 result; Fix64 c = FixMath.Cos(radians); Fix64 s = FixMath.Sin(radians); Fix64 x = centerPoint.x * (Fix64.One - c) - centerPoint.z * s; Fix64 z = centerPoint.x * (Fix64.One - c) + centerPoint.x * s; // [ c 0 -s 0 ] // [ 0 1 0 0 ] // [ s 0 c 0 ] // [ x 0 z 1 ] result.M11 = c; result.M12 = Fix64.Zero; result.M13 = -s; result.M14 = Fix64.Zero; result.M21 = Fix64.Zero; result.M22 = Fix64.One; result.M23 = Fix64.Zero; result.M24 = Fix64.Zero; result.M31 = s; result.M32 = Fix64.Zero; result.M33 = c; result.M34 = Fix64.Zero; result.M41 = x; result.M42 = Fix64.Zero; result.M43 = z; result.M44 = Fix64.One; return(result); }
/// <summary> /// Creates a matrix for rotating points around the Y-axis. /// </summary> /// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param> /// <returns>The rotation matrix.</returns> public static FixMatrix4x4 RotateY(Fix64 radians) { FixMatrix4x4 result; Fix64 c = FixMath.Cos(radians); Fix64 s = FixMath.Sin(radians); // [ c 0 -s 0 ] // [ 0 1 0 0 ] // [ s 0 c 0 ] // [ 0 0 0 1 ] result.M11 = c; result.M12 = Fix64.Zero; result.M13 = -s; result.M14 = Fix64.Zero; result.M21 = Fix64.Zero; result.M22 = Fix64.One; result.M23 = Fix64.Zero; result.M24 = Fix64.Zero; result.M31 = s; result.M32 = Fix64.Zero; result.M33 = c; result.M34 = Fix64.Zero; result.M41 = Fix64.Zero; result.M42 = Fix64.Zero; result.M43 = Fix64.Zero; result.M44 = Fix64.One; return(result); }
public FixVec2 Rotate(Fix degree) { var cos = FixMath.Cos(degree); var sin = FixMath.Sin(degree); return(new FixVec2(X * cos - Y * sin, X * sin + Y * cos)); }
/// <summary> /// Creates a matrix for rotating points around the X-axis, from a center point. /// </summary> /// <param name="radians">The amount, in radians, by which to rotate around the X-axis.</param> /// <param name="centerPoint">The center point.</param> /// <returns>The rotation matrix.</returns> public static FixMatrix4x4 RotateX(Fix64 radians, FixVector3 centerPoint) { FixMatrix4x4 result; Fix64 c = FixMath.Cos(radians); Fix64 s = FixMath.Sin(radians); Fix64 y = centerPoint.y * (Fix64.One - c) + centerPoint.z * s; Fix64 z = centerPoint.z * (Fix64.One - c) - centerPoint.y * s; // [ 1 0 0 0 ] // [ 0 c s 0 ] // [ 0 -s c 0 ] // [ 0 y z 1 ] result.M11 = Fix64.One; result.M12 = Fix64.Zero; result.M13 = Fix64.Zero; result.M14 = Fix64.Zero; result.M21 = Fix64.Zero; result.M22 = c; result.M23 = s; result.M24 = Fix64.Zero; result.M31 = Fix64.Zero; result.M32 = -s; result.M33 = c; result.M34 = Fix64.Zero; result.M41 = Fix64.Zero; result.M42 = y; result.M43 = z; result.M44 = Fix64.One; return(result); }
void LateUpdate() { if (position != old_position) { old_position = position; ApplyPosition(); } if (angle != old_angle) { if (lerpRotation) { lerpRotationTimes = 0; startRotation = transform_u3d.rotation; targetRotation = Quaternion.Euler(0, FixMath.AngleToDegree(angle), 0); } else { transform_u3d.rotation = Quaternion.Euler(0, FixMath.AngleToDegree(angle), 0); } old_angle = angle; } if (scale != old_scale) { old_scale = scale; ApplyScale(); } // lerp LerpRotation(); }
public GameSimulation(GameSettings setSttings, ConstData conConstantData) { m_setGameSettings = setSttings; m_conConstantGameData = conConstantData; //calculate queue length int iQueueLength = (int)FixMath.Ceiling(m_setGameSettings.m_fixTargetQueueLength / m_setGameSettings.m_fixTickDelta); //fill queue m_frmDenseFrameQueue = new List <FrameData>(iQueueLength); for (int i = 0; i < iQueueLength; i++) { m_frmDenseFrameQueue.Add(new FrameData(m_conConstantGameData.PlayerCount)); } m_bInputsForTick = new List <byte>(m_conConstantGameData.PlayerCount); m_ipbPlayerInputs = new List <InputBuffer>(m_conConstantGameData.PlayerCount); for (int i = 0; i < m_conConstantGameData.PlayerCount; i++) { m_bInputsForTick.Add(0); m_ipbPlayerInputs.Add(new InputBuffer(iQueueLength)); } }
public static void CatmullRom(ref FixVector2 value1, ref FixVector2 value2, ref FixVector2 value3, ref FixVector2 value4, Fix64 amount, out FixVector2 result) { result = new FixVector2( FixMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount), FixMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount)); }
public static void Barycentric(ref FixVector2 value1, ref FixVector2 value2, ref FixVector2 value3, Fix64 amount1, Fix64 amount2, out FixVector2 result) { result = new FixVector2( FixMath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2), FixMath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2)); }
// The smaller of the two possible angles between the two vectors is returned, therefore the result will never be greater than 180 degrees or smaller than -180 degrees. // If you imagine the from and to vectors as lines on a piece of paper, both originating from the same point, then the /axis/ vector would point up out of the paper. // The measured angle between the two vectors would be positive in a clockwise direction and negative in an anti-clockwise direction. public static Fix64 SignedAngle(FixVector3 from, FixVector3 to, FixVector3 axis) { FixVector3 fromNorm = from.Normalized, toNorm = to.Normalized; Fix64 unsignedAngle = FixMath.Acos(FixMath.Clamp(Dot(fromNorm, toNorm), -Fix64.ONE, Fix64.ONE)) * FixMath.Rad2Deg; Fix64 sign = FixMath.Sign(Dot(axis, Cross(fromNorm, toNorm))); return(unsignedAngle * sign); }
public static FixVector2 Lerp(FixVector2 value1, FixVector2 value2, Fix64 amount) { amount = FixMath.Clamp(amount, 0, 1); return(new FixVector2( FixMath.Lerp(value1.x, value2.x, amount), FixMath.Lerp(value1.y, value2.y, amount))); }
public static AABB Union(AABB a, AABB b) { AABB C; C.min = new FixVec2(FixMath.Min(a.min.x, b.min.x), FixMath.Min(a.min.y, b.min.y)); C.max = new FixVec2(FixMath.Max(a.max.x, b.max.x), FixMath.Max(a.max.y, b.max.y)); return(C); }
public void SqrtWorks(double value) { var expected = Math.Sqrt(value); var result = (double)FixMath.Sqrt((Fix)value); Assert.AreEqual(expected, result, (double)Fix.Epsilon * 4); }
public static Fix MatrixToDegrees(Mat22 m) { if (m.m10 == Fix.zero && m.m00 == Fix.zero) { return(Fix.zero); } return(FixMath.Atan2(m.m10, m.m00)); }
public static FixVec2 NormalizedToPoint(FixRect rectangle, FixVec2 normalizedRectCoordinates) { Fix x = FixMath.Min(FixMath.Max(Fix.Zero, normalizedRectCoordinates.X), Fix.One); Fix y = FixMath.Min(FixMath.Max(Fix.Zero, normalizedRectCoordinates.Y), Fix.One); return(new FixVec2(rectangle.xMin + (rectangle.xMax - rectangle.xMin) * x, rectangle.yMin + (rectangle.yMax - rectangle.yMin) * y)); }
public static AABB Union(AABB a, AABB b) { AABB C; C.min = new FixVec2(FixMath.Min(a.min.X, b.min.X), FixMath.Min(a.min.Y, b.min.Y)); C.max = new FixVec2(FixMath.Max(a.max.X, b.max.X), FixMath.Max(a.max.Y, b.max.Y)); return(C); }
public void CosWorks(double value) { var expected = Math.Cos(value * Math.PI / 180.0); var result = (double)FixMath.Cos((Fix)value); Assert.AreEqual(expected, result, (double)Fix.Epsilon); }
public static AABB Merge(AABB a, AABB b) { return(new AABB( FixMath.Min(a.L, b.L), FixMath.Min(a.B, b.B), FixMath.Max(a.R, b.R), FixMath.Max(a.T, b.T) )); }
public static AABB Expand(AABB aabb, FixVec2 v) { return(new AABB( FixMath.Min(aabb.L, v.X), FixMath.Min(aabb.B, v.Y), FixMath.Max(aabb.R, v.X), FixMath.Max(aabb.T, v.Y) )); }