/// <summary> /// Recursive algorithm base /// </summary> /// <param name="intersection">The intersection the recursive step started from</param> /// <param name="ray">The ray, starting from the intersection</param> /// <param name="scene">The scene to trace</param> private Color CalculateRecursiveColor(Intersection intersection, Scene scene, int depth) { // Ambient light: var color = Color.Lerp(Color.Black, intersection.Color * scene.AmbientLightColor, scene.AmbientLightIntensity); foreach (Light light in scene.Lights) { var lightContribution = new Color(); var towardsLight = (light.Position - intersection.Point).Normalized(); var lightDistance = Util.Distance(intersection.Point, light.Position); // Accumulate diffuse lighting: var lightEffectiveness = VectorMath.DotProduct(towardsLight, intersection.Normal); if (lightEffectiveness > 0.0f) { lightContribution = lightContribution + (intersection.Color * light.GetIntensityAtDistance(lightDistance) * light.Color * lightEffectiveness); } // Render shadow var shadowRay = new Ray(intersection.Point, towardsLight); Intersection shadowIntersection; if (TryCalculateIntersection(shadowRay, scene, intersection.ObjectHit, out shadowIntersection) && shadowIntersection.Distance < lightDistance) { var transparency = shadowIntersection.ObjectHit.Material.Transparency; var lightPassThrough = Util.Lerp(.25f, 1.0f, transparency); lightContribution = Color.Lerp(lightContribution, Color.Zero, 1 - lightPassThrough); } color += lightContribution; } if (depth < ReflectionDepth) { // Reflection ray var objectReflectivity = intersection.ObjectHit.Material.Reflectivity; if (objectReflectivity > 0.0f) { var reflectionRay = GetReflectionRay(intersection.Point, intersection.Normal, intersection.ImpactDirection); Intersection reflectionIntersection; if (TryCalculateIntersection(reflectionRay, scene, intersection.ObjectHit, out reflectionIntersection)) { color = Color.Lerp(color, CalculateRecursiveColor(reflectionIntersection, scene, depth + 1), objectReflectivity); } } // Refraction ray var objectRefractivity = intersection.ObjectHit.Material.Refractivity; if (objectRefractivity > 0.0f) { var refractionRay = GetRefractionRay(intersection.Point, intersection.Normal, intersection.ImpactDirection, objectRefractivity); Intersection refractionIntersection; if (TryCalculateIntersection(refractionRay, scene, intersection.ObjectHit, out refractionIntersection)) { var refractedColor = CalculateRecursiveColor(refractionIntersection, scene, depth + 1); color = Color.Lerp(color, refractedColor, 1 - (intersection.ObjectHit.Material.Opacity)); } } } color = color.Limited; return(color); }
//Apply friction to both vertical and horizontal momentum based on 'friction' and 'gravity'; //Handle sliding down steep slopes; void HandleMomentum() { //If local momentum is used, transform momentum into world coordinates first; if (useLocalMomentum) { momentum = tr.localToWorldMatrix * momentum; } Vector3 _verticalMomentum = Vector3.zero; Vector3 _horizontalMomentum = Vector3.zero; //Split momentum into vertical and horizontal components; if (momentum != Vector3.zero) { _verticalMomentum = VectorMath.ExtractDotVector(momentum, tr.up); _horizontalMomentum = momentum - _verticalMomentum; } //Add gravity to vertical momentum; _verticalMomentum -= tr.up * gravity * Time.deltaTime; //Remove any downward force if the controller is grounded; if (currentControllerState == ControllerState.Grounded) { _verticalMomentum = Vector3.zero; } //Apply friction to horizontal momentum based on whether the controller is grounded; if (currentControllerState == ControllerState.Grounded) { _horizontalMomentum = VectorMath.IncrementVectorLengthTowardTargetLength(_horizontalMomentum, groundFriction, Time.deltaTime, 0f); } else { _horizontalMomentum = VectorMath.IncrementVectorLengthTowardTargetLength(_horizontalMomentum, airFriction, Time.deltaTime, 0f); } //Add horizontal and vertical momentum back together; momentum = _horizontalMomentum + _verticalMomentum; //Project the current momentum onto the current ground normal if the controller is sliding down a slope; if (currentControllerState == ControllerState.Sliding) { momentum = Vector3.ProjectOnPlane(momentum, mover.GetGroundNormal()); } //Apply slide gravity along ground normal, if controller is sliding; if (currentControllerState == ControllerState.Sliding) { Vector3 _slideDirection = Vector3.ProjectOnPlane(-tr.up, mover.GetGroundNormal()).normalized; momentum += _slideDirection * slideGravity * Time.deltaTime; } //If controller is jumping, override vertical velocity with jumpSpeed; if (currentControllerState == ControllerState.Jumping) { momentum = VectorMath.RemoveDotVector(momentum, tr.up); momentum += tr.up * jumpSpeed; } if (useLocalMomentum) { momentum = tr.worldToLocalMatrix * momentum; } }
public Coords GetMovementVector(Occupant Occupant) { return(VectorMath.CalculateVector(Occupant.Position1, Occupant.Position2)); }
private void DrawGame(GameTime gameTime) { spriteBatch.Draw(backgroundTexture, new Rectangle(new Point(0, 0), Config.StaticResolution.ToPoint()), Color.White); // Player if (gameEngine.Player.IsAlive) { Vector2 speed = gameEngine.Player.CurrentSpeed; float angle = VectorMath.VectorToAngle(speed); // get direction angle AnimationSequence seq = (AnimationSequence)gameEngine.Player.Tag; seq.DestionationRectangle = gameEngine.Player.Hitbox; if (angle == MathHelper.ToRadians(-90) && seq.ActiveSequence.CurrentFrame == seq.ActiveSequence.StartIndex) // if angle = -90° and animation is finished { seq.Play(2); } else if (angle == MathHelper.ToRadians(-90) && seq.SequenceIndex == 1) // if angle = -90° and animation 1 is played { seq.ActiveSequence.Reversed = true; } if (angle == MathHelper.ToRadians(90) && seq.ActiveSequence.CurrentFrame == seq.ActiveSequence.StartIndex) // if angle = 90° and animation is finished { seq.Play(1); } else if (angle == MathHelper.ToRadians(90) && seq.SequenceIndex == 2) // if angle = -90° and animation 2 is played { seq.ActiveSequence.Reversed = true; } if (angle == MathHelper.Pi && (seq.SequenceIndex == 1 || seq.SequenceIndex == 2)) // if player is not moveing { seq.ActiveSequence.Reversed = true; } } else { } // Structures for (int i = 0; i < gameEngine.Structures.Structures.Length; i++) { Structure str = gameEngine.Structures.Structures[i]; if (!str.Destroyed) { if (str.RecentlyChanged) { structureTextures[i] = ImageProcessing.TextureFromBitmap(this.GraphicsDevice, str.StructureBitmap); str.RecentlyChanged = false; } spriteBatch.Draw(gameEngine.Structures.StructureTexture, str.Hitbox, Color.DimGray); spriteBatch.Draw(structureTextures[i], str.Hitbox, Color.White); } } // Enemies foreach (var e in gameEngine.Enemies.Enemies) { if (e.IsAlive && e.IsSpawned) { spriteBatch.Draw(enemyTexture, e.Hitbox, Color.White); } else if (!e.IsAlive && e.IsSpawned) { spriteBatch.Draw(enemyTexture, e.Hitbox, Color.Orange); } } // Particles particleEngine.Draw(spriteBatch); // Animations animationManager.Draw(spriteBatch); // Health string hp = gameEngine.Player.HealthPoints.ToString(); spriteBatch.DrawString(MenuFont_Text, hp, Utilities.GetContentAlign(gameEngine.Player.Hitbox, ContentAlignment.BottomCenter, MenuFont_Text.MeasureString(hp)), Color.Red); //Score string score = "Score: " + gameEngine.Score.Value.ToString("0.00"); spriteBatch.DrawString(MenuFont_Text, score, Utilities.GetContentAlign(new Rectangle(new Point(), Config.StaticResolution.ToPoint()), ContentAlignment.TopCenter, MenuFont_Text.MeasureString(score)), Color.Orange); // Projectiles if (gameEngine.Projectiles.Projectiles != null) { for (int i = 0; i < gameEngine.Projectiles.Projectiles.Length; i++) { Projectile proj = gameEngine.Projectiles.Projectiles[i]; if (!proj.IsExploded && !proj.OutOfBounds) { float angle = VectorMath.VectorToAngle(proj.Speed); Rectangle destRect = new Rectangle(proj.Location.X, proj.Location.Y, 30, 45); Vector2 origin = new Vector2(projectileTexture.Width / 2, projectileTexture.Height / 2); Color col = Color.White; if (proj.Type == Projectile.ProjectileType.Enemy_Proj_Def) { col = Color.LawnGreen; } spriteBatch.Draw(projectileTexture, destRect, null, col, angle, origin, SpriteEffects.None, 0); } } } if (gameEngine.State == Engine.EngineState.GameWon) { spriteBatch.Draw(endScreenTexture[0], new Rectangle(new Point(0, 0), Config.StaticResolution.ToPoint()), Color.White); } else if (gameEngine.State == Engine.EngineState.GameLost) { spriteBatch.Draw(endScreenTexture[1], new Rectangle(new Point(0, 0), Config.StaticResolution.ToPoint()), Color.White); } }
public override float ScaledDistance(Vector3 point) { return(VectorMath.Distance(Zone.Position, point) / Radius); }
/// <summary>Calculate an acceleration to move deltaPosition units and get there with approximately a velocity of targetVelocity</summary> public static Vector2 CalculateAccelerationToReachPoint(Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity, float forwardsAcceleration, float rotationSpeed, float maxSpeed, Vector2 forwardsVector) { // Guard against div by zero if (forwardsAcceleration <= 0) { return(Vector2.zero); } float currentSpeed = currentVelocity.magnitude; // Convert rotation speed to an acceleration // See https://en.wikipedia.org/wiki/Centripetal_force var sidewaysAcceleration = currentSpeed * rotationSpeed * Mathf.Deg2Rad; // To avoid weird behaviour when the rotation speed is very low we allow the agent to accelerate sideways without rotating much // if the rotation speed is very small. Also guards against division by zero. sidewaysAcceleration = Mathf.Max(sidewaysAcceleration, forwardsAcceleration); // Transform coordinates to local space where +X is the forwards direction // This is essentially equivalent to Transform.InverseTransformDirection. deltaPosition = VectorMath.ComplexMultiplyConjugate(deltaPosition, forwardsVector); targetVelocity = VectorMath.ComplexMultiplyConjugate(targetVelocity, forwardsVector); currentVelocity = VectorMath.ComplexMultiplyConjugate(currentVelocity, forwardsVector); float ellipseSqrFactorX = 1 / (forwardsAcceleration * forwardsAcceleration); float ellipseSqrFactorY = 1 / (sidewaysAcceleration * sidewaysAcceleration); // If the target velocity is zero we can use a more fancy approach // and calculate a nicer path. // In particular, this is the case at the end of the path. if (targetVelocity == Vector2.zero) { // Run a binary search over the time to get to the target point. float mn = 0.01f; float mx = 10; while (mx - mn > 0.01f) { var time = (mx + mn) * 0.5f; // Given that we want to move deltaPosition units from out current position, that our current velocity is given // and that when we reach the target we want our velocity to be zero. Also assume that our acceleration will // vary linearly during the slowdown. Then we can calculate what our acceleration should be during this frame. //{ t = time //{ deltaPosition = vt + at^2/2 + qt^3/6 //{ 0 = v + at + qt^2/2 //{ solve for a // a = acceleration vector // q = derivative of the acceleration vector var a = (6 * deltaPosition - 4 * time * currentVelocity) / (time * time); var q = 6 * (time * currentVelocity - 2 * deltaPosition) / (time * time * time); // Make sure the acceleration is not greater than our maximum allowed acceleration. // If it is we increase the time we want to use to get to the target // and if it is not, we decrease the time to get there faster. // Since the acceleration is described by acceleration = a + q*t // we only need to check at t=0 and t=time. // Note that the acceleration limit is described by an ellipse, not a circle. var nextA = a + q * time; if (a.x * a.x * ellipseSqrFactorX + a.y * a.y * ellipseSqrFactorY > 1.0f || nextA.x * nextA.x * ellipseSqrFactorX + nextA.y * nextA.y * ellipseSqrFactorY > 1.0f) { mn = time; } else { mx = time; } } var finalAcceleration = (6 * deltaPosition - 4 * mx * currentVelocity) / (mx * mx); // Boosting { // The trajectory calculated above has a tendency to use very wide arcs // and that does unfortunately not look particularly good in some cases. // Here we amplify the component of the acceleration that is perpendicular // to our current velocity. This will make the agent turn towards the // target quicker. // How much amplification to use. Value is unitless. const float Boost = 1; finalAcceleration.y *= 1 + Boost; // Clamp the velocity to the maximum acceleration. // Note that the maximum acceleration constraint is shaped like an ellipse, not like a circle. float ellipseMagnitude = finalAcceleration.x * finalAcceleration.x * ellipseSqrFactorX + finalAcceleration.y * finalAcceleration.y * ellipseSqrFactorY; if (ellipseMagnitude > 1.0f) { finalAcceleration /= Mathf.Sqrt(ellipseMagnitude); } } return(VectorMath.ComplexMultiply(finalAcceleration, forwardsVector)); } else { // Here we try to move towards the next waypoint which has been modified slightly using our // desired velocity at that point so that the agent will more smoothly round the corner. // How much to strive for making sure we reach the target point with the target velocity. Unitless. const float TargetVelocityWeight = 0.5f; // Limit to how much to care about the target velocity. Value is in seconds. // This prevents the character from moving away from the path too much when the target point is far away const float TargetVelocityWeightLimit = 1.5f; float targetSpeed; var normalizedTargetVelocity = VectorMath.Normalize(targetVelocity, out targetSpeed); var distance = deltaPosition.magnitude; var targetPoint = deltaPosition - normalizedTargetVelocity * System.Math.Min(TargetVelocityWeight * distance * targetSpeed / (currentSpeed + targetSpeed), maxSpeed * TargetVelocityWeightLimit); // How quickly the agent will try to reach the velocity that we want it to have. // We need this to prevent oscillations and jitter which is what happens if // we let the constant go towards zero. Value is in seconds. const float TimeToReachDesiredVelocity = 0.1f; // TODO: Clamp to ellipse using more accurate acceleration (use rotation speed as well) var finalAcceleration = (targetPoint.normalized * maxSpeed - currentVelocity) * (1f / TimeToReachDesiredVelocity); // Clamp the velocity to the maximum acceleration. // Note that the maximum acceleration constraint is shaped like an ellipse, not like a circle. float ellipseMagnitude = finalAcceleration.x * finalAcceleration.x * ellipseSqrFactorX + finalAcceleration.y * finalAcceleration.y * ellipseSqrFactorY; if (ellipseMagnitude > 1.0f) { finalAcceleration /= Mathf.Sqrt(ellipseMagnitude); } return(VectorMath.ComplexMultiply(finalAcceleration, forwardsVector)); } }
static void Main(string[] args) { var mag = VectorMath.Magnitude(new double[] { 7, -3, -9 }); Console.WriteLine("mag is {0}", mag); }
public void Vector2ToAngleException() { Vector <float> vector = new DenseVector(new[] { 0f, 1f, 2f }); Assert.That(() => VectorMath.Vector2ToAngle(vector), Throws.TypeOf <ArgumentException>()); }
internal void UpdateProperties() { Position = VectorMath.Transform(Vector3.Zero, Transform); }
public void VoxelizeInput(Pathfinding.Util.GraphTransform graphTransform, Bounds graphSpaceBounds) { AstarProfiler.StartProfile("Build Navigation Mesh"); AstarProfiler.StartProfile("Voxelizing - Step 1"); // Transform from voxel space to graph space. // then scale from voxel space (one unit equals one voxel) // Finally add min Matrix4x4 voxelMatrix = Matrix4x4.TRS(graphSpaceBounds.min, Quaternion.identity, Vector3.one) * Matrix4x4.Scale(new Vector3(cellSize, cellHeight, cellSize)); transformVoxel2Graph = new Pathfinding.Util.GraphTransform(voxelMatrix); // Transform from voxel space to world space // add half a voxel to fix rounding transform = graphTransform * voxelMatrix * Matrix4x4.TRS(new Vector3(0.5f, 0, 0.5f), Quaternion.identity, Vector3.one); int maximumVoxelYCoord = (int)(graphSpaceBounds.size.y / cellHeight); AstarProfiler.EndProfile("Voxelizing - Step 1"); AstarProfiler.StartProfile("Voxelizing - Step 2 - Init"); // Cosine of the slope limit in voxel space (some tweaks are needed because the voxel space might be stretched out along the y axis) float slopeLimit = Mathf.Cos(Mathf.Atan(Mathf.Tan(maxSlope * Mathf.Deg2Rad) * (cellSize / cellHeight))); // Temporary arrays used for rasterization var clipperOrig = new VoxelPolygonClipper(3); var clipperX1 = new VoxelPolygonClipper(7); var clipperX2 = new VoxelPolygonClipper(7); var clipperZ1 = new VoxelPolygonClipper(7); var clipperZ2 = new VoxelPolygonClipper(7); if (inputMeshes == null) { throw new System.NullReferenceException("inputMeshes not set"); } // Find the largest lengths of vertex arrays and check for meshes which can be skipped int maxVerts = 0; for (int m = 0; m < inputMeshes.Count; m++) { maxVerts = System.Math.Max(inputMeshes[m].vertices.Length, maxVerts); } // Create buffer, here vertices will be stored multiplied with the local-to-voxel-space matrix var verts = new Vector3[maxVerts]; AstarProfiler.EndProfile("Voxelizing - Step 2 - Init"); AstarProfiler.StartProfile("Voxelizing - Step 2"); // This loop is the hottest place in the whole rasterization process // it usually accounts for around 50% of the time for (int m = 0; m < inputMeshes.Count; m++) { RasterizationMesh mesh = inputMeshes[m]; var meshMatrix = mesh.matrix; // Flip the orientation of all faces if the mesh is scaled in such a way // that the face orientations would change // This happens for example if a mesh has a negative scale along an odd number of axes // e.g it happens for the scale (-1, 1, 1) but not for (-1, -1, 1) or (1,1,1) var flipOrientation = VectorMath.ReversesFaceOrientations(meshMatrix); Vector3[] vs = mesh.vertices; int[] tris = mesh.triangles; int trisLength = mesh.numTriangles; // Transform vertices first to world space and then to voxel space for (int i = 0; i < vs.Length; i++) { verts[i] = transform.InverseTransform(meshMatrix.MultiplyPoint3x4(vs[i])); } int mesharea = mesh.area; for (int i = 0; i < trisLength; i += 3) { Vector3 p1 = verts[tris[i]]; Vector3 p2 = verts[tris[i + 1]]; Vector3 p3 = verts[tris[i + 2]]; if (flipOrientation) { var tmp = p1; p1 = p3; p3 = tmp; } int minX = (int)(Utility.Min(p1.x, p2.x, p3.x)); int minZ = (int)(Utility.Min(p1.z, p2.z, p3.z)); int maxX = (int)System.Math.Ceiling(Utility.Max(p1.x, p2.x, p3.x)); int maxZ = (int)System.Math.Ceiling(Utility.Max(p1.z, p2.z, p3.z)); minX = Mathf.Clamp(minX, 0, voxelArea.width - 1); maxX = Mathf.Clamp(maxX, 0, voxelArea.width - 1); minZ = Mathf.Clamp(minZ, 0, voxelArea.depth - 1); maxZ = Mathf.Clamp(maxZ, 0, voxelArea.depth - 1); // Check if the mesh is completely out of bounds if (minX >= voxelArea.width || minZ >= voxelArea.depth || maxX <= 0 || maxZ <= 0) { continue; } Vector3 normal; int area; //AstarProfiler.StartProfile ("Rasterize..."); normal = Vector3.Cross(p2 - p1, p3 - p1); float cosSlopeAngle = Vector3.Dot(normal.normalized, Vector3.up); if (cosSlopeAngle < slopeLimit) { area = UnwalkableArea; } else { area = 1 + mesharea; } clipperOrig[0] = p1; clipperOrig[1] = p2; clipperOrig[2] = p3; clipperOrig.n = 3; for (int x = minX; x <= maxX; x++) { clipperOrig.ClipPolygonAlongX(ref clipperX1, 1f, -x + 0.5f); if (clipperX1.n < 3) { continue; } clipperX1.ClipPolygonAlongX(ref clipperX2, -1F, x + 0.5F); if (clipperX2.n < 3) { continue; } float clampZ1 = clipperX2.z[0]; float clampZ2 = clipperX2.z[0]; for (int q = 1; q < clipperX2.n; q++) { float val = clipperX2.z[q]; clampZ1 = System.Math.Min(clampZ1, val); clampZ2 = System.Math.Max(clampZ2, val); } int clampZ1I = Mathf.Clamp((int)System.Math.Round(clampZ1), 0, voxelArea.depth - 1); int clampZ2I = Mathf.Clamp((int)System.Math.Round(clampZ2), 0, voxelArea.depth - 1); for (int z = clampZ1I; z <= clampZ2I; z++) { clipperX2.ClipPolygonAlongZWithYZ(ref clipperZ1, 1F, -z + 0.5F); if (clipperZ1.n < 3) { continue; } clipperZ1.ClipPolygonAlongZWithY(ref clipperZ2, -1F, z + 0.5F); if (clipperZ2.n < 3) { continue; } float sMin = clipperZ2.y[0]; float sMax = clipperZ2.y[0]; for (int q = 1; q < clipperZ2.n; q++) { float val = clipperZ2.y[q]; sMin = System.Math.Min(sMin, val); sMax = System.Math.Max(sMax, val); } int maxi = (int)System.Math.Ceiling(sMax); // Skip span if below or above the bounding box if (maxi >= 0 && sMin <= maximumVoxelYCoord) { // Make sure mini >= 0 int mini = System.Math.Max(0, (int)sMin); // Make sure the span is at least 1 voxel high maxi = System.Math.Max(mini + 1, maxi); voxelArea.AddLinkedSpan(z * voxelArea.width + x, (uint)mini, (uint)maxi, area, voxelWalkableClimb); } } } } //AstarProfiler.EndFastProfile(0); //AstarProfiler.EndProfile ("Rasterize..."); } AstarProfiler.EndProfile("Voxelizing - Step 2"); }
/// <summary> /// Aggregate the valuation profile onto a set of result curves to support result partitioning. /// </summary> protected override void ProcessResults(ValuationResults valResults, DealPartitionAssociations assoc, PriceFactorList factors, BaseTimeGrid baseTimes, ValuationOptions options, int partition) { var pvProfiles = valResults.Results <PVProfiles>(); var addOnProfiles = valResults.Results <AddOnProfiles>(); var positiveMtmProfiles = valResults.Results <PositiveMtmProfiles>(); Debug.Assert(addOnProfiles != null, "No Add-On profiles. Cannot proceed with valuation."); Debug.Assert(positiveMtmProfiles != null, "No Positive mtM profiles. Cannot proceed with valuation."); fT = Deal.ValuationGrid(factors, baseTimes, Deal.EndDate()); var tgi = new TimeGridIterator(fT); var nettedExposure = new PVProfiles(factors.NumScenarios); var collateralExposure = new PVProfiles(factors.NumScenarios); var addOnsProfile = new PVProfiles(factors.NumScenarios); var mtmTermProfile = new PVProfiles(factors.NumScenarios); DealBaselNettingCollateralSet nettingSetDeal = Deal as DealBaselNettingCollateralSet; bool collateralised = nettingSetDeal.Collateralised == YesNo.Yes; using (var cache = Vector.Cache(factors.NumScenarios)) { Vector sumMtm = cache.Get(); Vector sumPositiveMtm = cache.Get(); Vector addOns = cache.Get(); Vector netGrossRatio = cache.Get(); Vector value = cache.Get(); Vector term1 = cache.Get(); Vector term2 = cache.Get(); // Collateral related vectors. Vector mtmTermStart = cache.Get(); Vector addOnHp = cache.Get(); // Loop to get the netting set exposure. while (tgi.Next()) { sumMtm.Clear(); sumPositiveMtm.Clear(); addOns.Clear(); value.Clear(); double date = tgi.Date; // For MtM Plus Add-On deals PV profiles represents the sum of the MtM profile and Add-On profile. // Subtract the Add-On profile to recover the MtM profile before flooring. sumMtm.Assign(VectorMath.Max(pvProfiles[date] - addOnProfiles[date], 0.0)); addOns.Assign(addOnProfiles[date]); sumPositiveMtm.Assign(positiveMtmProfiles[date]); netGrossRatio.AssignConditional(sumPositiveMtm > 0, sumMtm / sumPositiveMtm, 0.0); netGrossRatio.MultiplyBy(this.fNetGrossRatioCorrelation); netGrossRatio.Add(1 - this.fNetGrossRatioCorrelation); term2.AssignProduct(addOns, netGrossRatio); term1.Assign(VectorMath.Max(sumMtm, 0.0)); value.AssignSum(term1, term2); nettedExposure.AppendVector(date, value); if (collateralised) { mtmTermProfile.AppendVector(date, term1); addOnsProfile.AppendVector(date, term2); } } nettedExposure.Complete(this.fT); var exposureResults = valResults.Results <Exposure>(); if (exposureResults != null) { exposureResults.Assign(nettedExposure); } // Collateral cases. if (collateralised) { mtmTermProfile.Complete(this.fT); addOnsProfile.Complete(this.fT); double date = factors.BaseDate; mtmTermProfile.GetValue(mtmTermStart, date); addOnsProfile.GetValue(addOnHp, date + nettingSetDeal.Holding_Period); // Assume we have post haircut collateral. double collateral = nettingSetDeal.Balance; double threshold = nettingSetDeal.Threshold; tgi.Reset(); // Loop to get the netting set exposure. while (tgi.Next()) { bool inHoldingPeriod = tgi.T < CalcUtils.DaysToYears(nettingSetDeal.Holding_Period); CollateralBasel3(mtmTermStart, collateral, addOnHp, threshold, inHoldingPeriod, tgi.Date, nettedExposure, collateralExposure); } collateralExposure.Complete(this.fT); if (exposureResults != null) { exposureResults.Assign(collateralExposure); } } } if (options.PartitionCollateralMode != PartitionCollateralMode.Suppress_Collateral_And_Flooring || partition < options.NumTotalPartitions) { valResults.FloorResult(assoc.AggregationMode, options); } CollateralPlugIn.CollateralBalancesContainer coProfiles = valResults.Results <CollateralPlugIn.CollateralBalancesContainer>(); // Store collateral information according to diagnostic collection rules. if (coProfiles != null) { coProfiles.StoreInformation(this); } }
private bool IsRisingOrFalling() { var verticalMomentum = VectorMath.ExtractDotVector(momentum, mover.transform.up); return(verticalMomentum.magnitude > 0.001f); }
private void HandleState() { var isRising = IsRisingOrFalling() && (VectorMath.GetDotProduct(momentum, mover.transform.up) > 0f); var isSliding = mover.IsGrounded() && (Vector3.Angle(mover.GetGroundNormal(), mover.transform.up) > motorVariables.SlopeLimit); switch (currentMotorState) { case PlayerMotorStateEnum.Grounded: if (isRising) { currentMotorState = PlayerMotorStateEnum.Rising; GroundContactLost(); break; } if (!mover.IsGrounded()) { currentMotorState = PlayerMotorStateEnum.Falling; GroundContactLost(); break; } if (isSliding) { currentMotorState = PlayerMotorStateEnum.Sliding; break; } lastGroundedTime = Time.time; break; case PlayerMotorStateEnum.Falling: if (isRising) { currentMotorState = PlayerMotorStateEnum.Rising; break; } if (mover.IsGrounded() && !isSliding) { currentMotorState = PlayerMotorStateEnum.Grounded; GroundContactRegained(momentum); break; } if (isSliding) { currentMotorState = PlayerMotorStateEnum.Sliding; GroundContactRegained(momentum); break; } break; case PlayerMotorStateEnum.Sliding: if (isRising) { currentMotorState = PlayerMotorStateEnum.Rising; GroundContactLost(); break; } if (!mover.IsGrounded()) { currentMotorState = PlayerMotorStateEnum.Falling; break; } if (mover.IsGrounded() && !isSliding) { GroundContactRegained(momentum); currentMotorState = PlayerMotorStateEnum.Grounded; break; } break; case PlayerMotorStateEnum.Rising: if (isRising) { break; } if (mover.IsGrounded() && !isSliding) { currentMotorState = PlayerMotorStateEnum.Grounded; GroundContactRegained(momentum); break; } if (isSliding) { currentMotorState = PlayerMotorStateEnum.Sliding; break; } if (!mover.IsGrounded()) { currentMotorState = PlayerMotorStateEnum.Falling; break; } break; case PlayerMotorStateEnum.Jumping: if ((Time.time - jumpStartTime) > motorVariables.JumpDuration) { currentMotorState = PlayerMotorStateEnum.Rising; break; } break; } }
/** \todo This is the area in XZ space, use full 3D space for higher correctness maybe? */ public override float SurfaceArea() { var holder = GetNavmeshHolder(GraphIndex); return(System.Math.Abs(VectorMath.SignedTriangleAreaTimes2XZ(holder.GetVertex(v0), holder.GetVertex(v1), holder.GetVertex(v2))) * 0.5f); }
/// <summary>Update is called once per frame</summary> protected override void Update() { deltaTime = Mathf.Min(Time.smoothDeltaTime * 2, Time.deltaTime); if (richPath != null) { //System.Diagnostics.Stopwatch w = new System.Diagnostics.Stopwatch(); //w.Start(); RichPathPart pt = richPath.GetCurrentPart(); var fn = pt as RichFunnel; if (fn != null) { //Clear buffers for reuse Vector3 position = UpdateTarget(fn); //tr.position = ps; //Only get walls every 5th frame to save on performance if (Time.frameCount % 5 == 0 && wallForce > 0 && wallDist > 0) { wallBuffer.Clear(); fn.FindWalls(wallBuffer, wallDist); } /*for (int i=0;i<wallBuffer.Count;i+=2) { * Debug.DrawLine (wallBuffer[i],wallBuffer[i+1],Color.magenta); * }*/ //Pick next waypoint if current is reached int tgIndex = 0; /*if (buffer.Count > 1) { * if ((buffer[tgIndex]-tr.position).sqrMagnitude < pickNextWaypointDist*pickNextWaypointDist) { * tgIndex++; * } * }*/ //Target point Vector3 tg = nextCorners[tgIndex]; Vector3 dir = tg - position; dir.y = 0; bool passedTarget = Vector3.Dot(dir, currentTargetDirection) < 0; //Check if passed target in another way if (passedTarget && nextCorners.Count - tgIndex > 1) { tgIndex++; tg = nextCorners[tgIndex]; } if (tg != lastTargetPoint) { currentTargetDirection = (tg - position); currentTargetDirection.y = 0; currentTargetDirection.Normalize(); lastTargetPoint = tg; //Debug.DrawRay (tr.position, Vector3.down*2,Color.blue,0.2f); } //Direction to target dir = (tg - position); dir.y = 0; float magn = dir.magnitude; //Write out for other scripts to read distanceToSteeringTarget = magn; //Normalize dir = magn == 0 ? Vector3.zero : dir / magn; Vector3 normdir = dir; Vector3 force = Vector3.zero; if (wallForce > 0 && wallDist > 0) { float wLeft = 0; float wRight = 0; for (int i = 0; i < wallBuffer.Count; i += 2) { Vector3 closest = VectorMath.ClosestPointOnSegment(wallBuffer[i], wallBuffer[i + 1], tr.position); float dist = (closest - position).sqrMagnitude; if (dist > wallDist * wallDist) { continue; } Vector3 tang = (wallBuffer[i + 1] - wallBuffer[i]).normalized; //Using the fact that all walls are laid out clockwise (seeing from inside) //Then left and right (ish) can be figured out like this float dot = Vector3.Dot(dir, tang) * (1 - System.Math.Max(0, (2 * (dist / (wallDist * wallDist)) - 1))); if (dot > 0) { wRight = System.Math.Max(wRight, dot); } else { wLeft = System.Math.Max(wLeft, -dot); } } Vector3 norm = Vector3.Cross(Vector3.up, dir); force = norm * (wRight - wLeft); //Debug.DrawRay (tr.position, force, Color.cyan); } //Is the endpoint of the path (part) the current target point bool endPointIsTarget = lastCorner && nextCorners.Count - tgIndex == 1; if (endPointIsTarget) { //Use 2nd or 3rd degree motion equation to figure out acceleration to reach target in "exact" [slowdownTime] seconds //Clamp to avoid divide by zero if (slowdownTime < 0.001f) { slowdownTime = 0.001f; } Vector3 diff = tg - position; diff.y = 0; if (preciseSlowdown) { //{ t = slowdownTime //{ diff = vt + at^2/2 + qt^3/6 //{ 0 = at + qt^2/2 //{ solve for a dir = (6 * diff - 4 * slowdownTime * velocity) / (slowdownTime * slowdownTime); } else { dir = 2 * (diff - slowdownTime * velocity) / (slowdownTime * slowdownTime); } dir = Vector3.ClampMagnitude(dir, acceleration); force *= System.Math.Min(magn / 0.5f, 1); if (magn < endReachedDistance) { //END REACHED NextPart(); } } else { dir *= acceleration; } //Debug.DrawRay (tr.position+Vector3.up, dir*3, Color.blue); velocity += (dir + force * wallForce) * deltaTime; if (slowWhenNotFacingTarget) { float dot = (Vector3.Dot(normdir, tr.forward) + 0.5f) * (1.0f / 1.5f); //velocity = Vector3.ClampMagnitude (velocity, maxSpeed * Mathf.Max (dot, 0.2f) ); float xzmagn = Mathf.Sqrt(velocity.x * velocity.x + velocity.z * velocity.z); float prevy = velocity.y; velocity.y = 0; float mg = Mathf.Min(xzmagn, maxSpeed * Mathf.Max(dot, 0.2f)); velocity = Vector3.Lerp(tr.forward * mg, velocity.normalized * mg, Mathf.Clamp(endPointIsTarget ? (magn * 2) : 0, 0.5f, 1.0f)); velocity.y = prevy; } else { // Clamp magnitude on the XZ axes float xzmagn = Mathf.Sqrt(velocity.x * velocity.x + velocity.z * velocity.z); xzmagn = maxSpeed / xzmagn; if (xzmagn < 1) { velocity.x *= xzmagn; velocity.z *= xzmagn; //Vector3.ClampMagnitude (velocity, maxSpeed); } } //Debug.DrawLine (tr.position, tg, lastCorner ? Color.red : Color.green); if (endPointIsTarget) { Vector3 trotdir = Vector3.Lerp(velocity, currentTargetDirection, System.Math.Max(1 - magn * 2, 0)); RotateTowards(trotdir); } else { RotateTowards(velocity); } //Applied after rotation to enable proper checks on if velocity is zero velocity += deltaTime * gravity; if (rvoController != null && rvoController.enabled) { //Use RVOController tr.position = position; rvoController.Move(velocity); } else if (controller != null && controller.enabled) { //Use CharacterController tr.position = position; controller.Move(velocity * deltaTime); } else { //Use Transform float lasty = position.y; position += velocity * deltaTime; position = RaycastPosition(position, lasty); tr.position = position; } } else { if (rvoController != null && rvoController.enabled) { //Use RVOController rvoController.Move(Vector3.zero); } } if (pt is RichSpecial) { if (!traversingOffMeshLink) { StartCoroutine(TraverseSpecial(pt as RichSpecial)); } } //w.Stop(); //Debug.Log ((w.Elapsed.TotalMilliseconds*1000)); } else { if (rvoController != null && rvoController.enabled) { //Use RVOController rvoController.Move(Vector3.zero); } else if (controller != null && controller.enabled) { } else { tr.position = RaycastPosition(tr.position, tr.position.y); } } UpdateVelocity(); lastDeltaTime = Time.deltaTime; }
public void SetTransform(Matrix4 transform) { RigidBody.WorldTransform = transform.Cast(); Position = VectorMath.Transform(Vector3.Zero, Transform); }
//public Vector2D Calculate() //{ // Vector2D force; // My force will be stored here // Vector2D pos = _me.Pos; // Position of the agent // // For each wall // for (int j = 0; j < _me.Walls.Count(); j++) // { // var wall = _me.Walls[j]; // var x = (wall.Center.X + _me.Pos.X) / 2; // var y = (wall.Center.Y + _me.Pos.Y) / 2; // Vector2D distance = new Vector2D(x, y); // // If the wall is visible, calculate the force to apply // double dotProduct = distance * partsList[j]->normal(); // if (dotProduct < 0) // { // force += partsList[j]->normal() / (distance.length() * distance.length() + 1); // } // } // // Returned the calculated force // return force; //} public IWall GetClosestWall(IWallAvoider ME) { IWall mostThreatening = null; for (int i = 0; i < ME.Walls.Count(); i++) { IWall wall = ME.Walls[i]; bool collision = findSensorCollision(wall); if (collision && (mostThreatening == null || VectorMath.DistanceBetweenPositions(ME.Pos, wall.Center) < VectorMath.DistanceBetweenPositions(ME.Pos, mostThreatening.Center))) { mostThreatening = wall; } } return(mostThreatening); }
public Vector3 RotateVector(Vector3 src) { return(VectorMath.Transform(src, RigidBody.WorldTransform.Cast().ClearTranslation())); }
public override bool LineCastForMoving(ref HitInfo hit, MoveType mov) { Int3 from = hit.from; Int3 to = hit.to; hit.hitPosition = from; if (trace != null) { trace.Clear(); } Int3 end = to; Int3 origin = from; if (origin == end) { hit.hitPosition = from; return(false); } NavMeshNode node = GetNearest(from, NNConstraint.None).node; if (node == null) { Debug.LogError("Could not find a valid node to start from"); hit.hitPosition = from; return(true); } origin = node.ClosestPointOnNode(origin); List <Int3> left = Util.ListPool <Int3> .Claim(); List <Int3> right = Util.ListPool <Int3> .Claim(); int counter = 0; while (true) { counter++; if (counter > 2000) { Debug.LogError("Linecast was stuck in infinite loop. Breaking."); Util.ListPool <Int3> .Release(left); Util.ListPool <Int3> .Release(right); return(true); } NavMeshNode newNode = null; if (trace != null) { trace.Add(node); } if (node.ContainsPoint(end)) { hit.hitPosition = to; Util.ListPool <Int3> .Release(left); Util.ListPool <Int3> .Release(right); return(false); } for (int i = 0; i < node.connections.Length; i++) { left.Clear(); right.Clear(); NavMeshNode other = GetNode(node.connections[i]) as NavMeshNode; if (!node.GetPortal(other, left, right)) { continue; } Int3 a = left[0]; Int3 b = right[0]; //i.e Left or colinear if (!VectorMath.RightXZ(a, b, origin)) { if (VectorMath.RightXZ(a, b, end)) { //Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it continue; } } float factor1, factor2; if (VectorMath.LineIntersectionFactorXZ(a, b, origin, end, out factor1, out factor2)) { //Intersection behind the start if (factor2 < 0) { continue; } if (factor1 >= 0 && factor1 <= 1) { newNode = other; break; } } } if (newNode == null) { //Possible edge hit int vs = node.GetVertexCount(); for (int i = 0; i < vs; i++) { var a = node.GetVertex(i); var b = node.GetVertex((i + 1) % vs); //i.e left or colinear if (!VectorMath.RightXZ(a, b, origin)) { //Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it if (VectorMath.RightXZ(a, b, end)) { //Since polygons are laid out in clockwise order, the ray would intersect (if intersecting) this edge going in to the node, not going out from it continue; } } float factor1, factor2; if (VectorMath.LineIntersectionFactorXZ(a, b, origin, end, out factor1, out factor2)) { if (factor2 < 0) { continue; } if (factor1 >= 0 && factor1 <= 1) { Vector3 intersectionPoint = (Vector3)a + (Vector3)(b - a) * factor1; hit.hitPosition = new Int3(intersectionPoint); Util.ListPool <Int3> .Release(left); Util.ListPool <Int3> .Release(right); return(true); } } } //Ok, this is wrong... Debug.LogWarning("Linecast failing because point not inside node, and line does not hit any edges of it"); Util.ListPool <Int3> .Release(left); Util.ListPool <Int3> .Release(right); return(false); } node = newNode; } }
public override float ScaledDistance(Vector3 point) { var max = Math.Max(Math.Max(Size.X, Size.Y), Size.Z); return(VectorMath.Distance(transformedPos, point) / max); }
public override float ScaledDistance(Vector3 point) { return(VectorMath.Distance(point, centre) / Radius); }
private IObstacle findMostThreateningObstacle(IObstacleAvoider me) { IObstacle mostThreatening = null; for (int i = 0; i < me.Obstacles.Count(); i++) { IObstacle obstacle = me.Obstacles[i]; bool collision = lineIntersectsCircleAhead(obstacle); if (collision && (mostThreatening == null || VectorMath.DistanceBetweenPositions(me.Pos, obstacle.Center) < VectorMath.DistanceBetweenPositions(me.Pos, mostThreatening.Center))) { mostThreatening = obstacle; } } return(mostThreatening); }
// Start is called before the first frame update void Start() { distance = VectorMath.SqrDistanceXZ(transform.position, target.position); direction = (target.position - transform.position); returnTarget = transform.position; }
private bool lineIntersectsCircleAhead(IObstacle obstacle) { return(VectorMath.DistanceBetweenPositions(obstacle.Center, _ahead) <= obstacle.Radius || VectorMath.DistanceBetweenPositions(obstacle.Center, _ahead2) <= obstacle.Radius); }
//Handle state transitions; //Determine current controller state based on current momentum and whether the controller is grounded (or not); void HandleState() { //Check if vertical momentum is pointing upwards; bool _isRising = IsRisingOrFalling() && (VectorMath.GetDotProduct(GetMomentum(), tr.up) > 0f); //Check if controller is sliding; bool _isSliding = mover.IsGrounded() && IsGroundTooSteep(); switch (currentControllerState) { case ControllerState.Grounded: if (_isRising) { currentControllerState = ControllerState.Rising; OnGroundContactLost(); break; } if (!mover.IsGrounded()) { currentControllerState = ControllerState.Falling; OnGroundContactLost(); break; } if (_isSliding) { currentControllerState = ControllerState.Sliding; break; } break; case ControllerState.Falling: if (_isRising) { currentControllerState = ControllerState.Rising; break; } if (mover.IsGrounded() && !_isSliding) { currentControllerState = ControllerState.Grounded; OnGroundContactRegained(momentum); break; } if (_isSliding) { currentControllerState = ControllerState.Sliding; OnGroundContactRegained(momentum); break; } break; case ControllerState.Sliding: if (_isRising) { currentControllerState = ControllerState.Rising; OnGroundContactLost(); break; } if (!mover.IsGrounded()) { currentControllerState = ControllerState.Falling; break; } if (mover.IsGrounded() && !_isSliding) { OnGroundContactRegained(momentum); currentControllerState = ControllerState.Grounded; break; } break; case ControllerState.Rising: if (!_isRising) { if (mover.IsGrounded() && !_isSliding) { currentControllerState = ControllerState.Grounded; OnGroundContactRegained(momentum); break; } if (_isSliding) { currentControllerState = ControllerState.Sliding; break; } if (!mover.IsGrounded()) { currentControllerState = ControllerState.Falling; break; } } break; case ControllerState.Jumping: //Check for jump timeout; if ((Time.time - currentJumpStartTime) > jumpDuration) { currentControllerState = ControllerState.Rising; } //Check if jump key was let go; if (jumpKeyWasLetGo) { currentControllerState = ControllerState.Rising; } break; } }
private ColorPacket256 GetNaturalColor(Vector256 <int> things, VectorPacket256 pos, VectorPacket256 norms, VectorPacket256 rds, Scene scene) { var colors = ColorPacket256Helper.DefaultColor; for (int i = 0; i < scene.Lights.Length; i++) { var lights = scene.Lights[i]; var zero = Vector256 <float> .Zero; var colorPacket = lights.Colors; var ldis = lights.Positions - pos; var livec = ldis.Normalize(); var neatIsectDis = TestRay(new RayPacket256(pos, livec), scene); // is in shadow? var mask1 = Compare(neatIsectDis, ldis.Lengths, FloatComparisonMode.OrderedLessThanOrEqualNonSignaling); var mask2 = Compare(neatIsectDis, zero, FloatComparisonMode.OrderedNotEqualNonSignaling); var isInShadow = And(mask1, mask2); Vector256 <float> illum = VectorPacket256.DotProduct(livec, norms); Vector256 <float> illumGraterThanZero = Compare(illum, zero, FloatComparisonMode.OrderedGreaterThanNonSignaling); var tmpColor1 = illum * colorPacket; var defaultRGB = zero; Vector256 <float> lcolorR = BlendVariable(defaultRGB, tmpColor1.Xs, illumGraterThanZero); Vector256 <float> lcolorG = BlendVariable(defaultRGB, tmpColor1.Ys, illumGraterThanZero); Vector256 <float> lcolorB = BlendVariable(defaultRGB, tmpColor1.Zs, illumGraterThanZero); ColorPacket256 lcolor = new ColorPacket256(lcolorR, lcolorG, lcolorB); Vector256 <float> specular = VectorPacket256.DotProduct(livec, rds.Normalize()); Vector256 <float> specularGraterThanZero = Compare(specular, zero, FloatComparisonMode.OrderedGreaterThanNonSignaling); var difColor = new ColorPacket256(1, 1, 1); var splColor = new ColorPacket256(1, 1, 1); var roughness = Vector256.Create(1.0f); for (int j = 0; j < scene.Things.Length; j++) { Vector256 <float> thingMask = CompareEqual(things, Vector256.Create(j)).AsSingle(); Vector256 <float> rgh = Vector256.Create(scene.Things[j].Surface.Roughness); var dif = scene.Things[j].Surface.Diffuse(pos); var spl = scene.Things[j].Surface.Specular; roughness = BlendVariable(roughness, rgh, thingMask); difColor.Xs = BlendVariable(difColor.Xs, dif.Xs, thingMask); difColor.Ys = BlendVariable(difColor.Ys, dif.Ys, thingMask); difColor.Zs = BlendVariable(difColor.Zs, dif.Zs, thingMask); splColor.Xs = BlendVariable(splColor.Xs, spl.Xs, thingMask); splColor.Ys = BlendVariable(splColor.Ys, spl.Ys, thingMask); splColor.Zs = BlendVariable(splColor.Zs, spl.Zs, thingMask); } var tmpColor2 = VectorMath.Pow(specular, roughness) * colorPacket; Vector256 <float> scolorR = BlendVariable(defaultRGB, tmpColor2.Xs, specularGraterThanZero); Vector256 <float> scolorG = BlendVariable(defaultRGB, tmpColor2.Ys, specularGraterThanZero); Vector256 <float> scolorB = BlendVariable(defaultRGB, tmpColor2.Zs, specularGraterThanZero); ColorPacket256 scolor = new ColorPacket256(scolorR, scolorG, scolorB); var oldColor = colors; colors = colors + ColorPacket256Helper.Times(difColor, lcolor) + ColorPacket256Helper.Times(splColor, scolor); colors = new ColorPacket256(BlendVariable(colors.Xs, oldColor.Xs, isInShadow), BlendVariable(colors.Ys, oldColor.Ys, isInShadow), BlendVariable(colors.Zs, oldColor.Zs, isInShadow)); } return(colors); }
/** Calculate an acceleration to move deltaPosition units and get there with approximately a velocity of targetVelocity */ public static Vector2 CalculateAccelerationToReachPoint(Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity, float acceleration, float maxSpeed) { // If the target velocity is zero we can use a more fancy approach // and calculate a nicer path. // In particular, this is the case at the end of the path. if (targetVelocity == Vector2.zero) { // Run a binary search over the time to get to the target point. float mn = 0.01f; float mx = 10; float sqrAcceleration = acceleration * acceleration; while (mx - mn > 0.01f) { var time = (mx + mn) * 0.5f; // Given that we want to move deltaPosition units from out current position, that our current velocity is given // and that when we reach the target we want our velocity to be zero. Also assume that our acceleration will // vary linearly during the slowdown. Then we can calculate what our acceleration should be during this frame. //{ t = time //{ deltaPosition = vt + at^2/2 + qt^3/6 //{ 0 = v + at + qt^2/2 //{ solve for a // a = acceleration vector // q = derivative of the acceleration vector var a = (6 * deltaPosition - 4 * time * currentVelocity) / (time * time); var q = 6 * (time * currentVelocity - 2 * deltaPosition) / (time * time * time); // Make sure the acceleration is not greater than our maximum allowed acceleration. // If it is we increase the time we want to use to get to the target // and if it is not, we decrease the time to get there faster. // Since the acceleration is described by acceleration = a + q*t // we only need to check at t=0 and t=time. if (a.sqrMagnitude > sqrAcceleration || (a + q * time).sqrMagnitude > sqrAcceleration) { mn = time; } else { mx = time; } } var finalAcceleration = (6 * deltaPosition - 4 * mx * currentVelocity) / (mx * mx); // The trajectory calculated above has a tendency to use very wide arcs // and that does unfortunately not look particularly good in some cases. // Here we amplify the component of the acceleration that is perpendicular // to our current velocity. This will make the agent turn towards the // target quicker. var right = new Vector2(currentVelocity.y, -currentVelocity.x); // How much amplification to use. Value is unitless. const float Boost = 1f; finalAcceleration = Vector3.ClampMagnitude(finalAcceleration + (Boost * Vector3.Dot(finalAcceleration, right) / Mathf.Max(0.0001f, right.sqrMagnitude)) * right, acceleration); return(finalAcceleration); } else { // Here we try to move towards the next waypoint which has been modified slightly using our // desired velocity at that point so that the agent will more smoothly round the corner. // How much to strive for making sure we reach the target point with the target velocity. Unitless. const float TargetVelocityWeight = 0.5f; // Limit to how much to care about the target velocity. Value is in seconds. // This prevents the character from moving away from the path too much when the target point is far away const float TargetVelocityWeightLimit = 1.5f; float currentSpeed = currentVelocity.magnitude; float targetSpeed; var normalizedTargetVelocity = VectorMath.Normalize(targetVelocity, out targetSpeed); var distance = deltaPosition.magnitude; var targetPoint = deltaPosition - normalizedTargetVelocity * System.Math.Min(TargetVelocityWeight * distance * targetSpeed / (currentSpeed + targetSpeed), maxSpeed * TargetVelocityWeightLimit); // How quickly the agent will try to reach the velocity that we want it to have. // We need this to prevent oscillations and jitter which is what happens if // we let the constant go towards zero. Value is in seconds. const float TimeToReachDesiredVelocity = 0.1f; // TODO: Clamp to ellipse using more accurate acceleration (use rotation speed as well) return(Vector2.ClampMagnitude((targetPoint.normalized * maxSpeed - currentVelocity) * (1f / TimeToReachDesiredVelocity), acceleration)); } }
public void Update() { if (Time.time >= nextRepath && canSearchAgain) { RecalculatePath(); } Vector3 pos = transform.position; if (vectorPath != null && vectorPath.Count != 0) { while ((controller.To2D(pos - vectorPath[wp]).sqrMagnitude < moveNextDist * moveNextDist && wp != vectorPath.Count - 1) || wp == 0) { wp++; } // Current path segment goes from vectorPath[wp-1] to vectorPath[wp] // We want to find the point on that segment that is 'moveNextDist' from our current position. // This can be visualized as finding the intersection of a circle with radius 'moveNextDist' // centered at our current position with that segment. var p1 = vectorPath[wp - 1]; var p2 = vectorPath[wp]; // Calculate the intersection with the circle. This involves some math. var t = VectorMath.LineCircleIntersectionFactor(controller.To2D(transform.position), controller.To2D(p1), controller.To2D(p2), moveNextDist); // Clamp to a point on the segment t = Mathf.Clamp01(t); Vector3 waypoint = Vector3.Lerp(p1, p2, t); // Calculate distance to the end of the path float remainingDistance = controller.To2D(waypoint - pos).magnitude + controller.To2D(waypoint - p2).magnitude; for (int i = wp; i < vectorPath.Count - 1; i++) { remainingDistance += controller.To2D(vectorPath[i + 1] - vectorPath[i]).magnitude; } // Set the target to a point in the direction of the current waypoint at a distance // equal to the remaining distance along the path. Since the rvo agent assumes that // it should stop when it reaches the target point, this will produce good avoidance // behavior near the end of the path. When not close to the end point it will act just // as being commanded to move in a particular direction, not toward a particular point var rvoTarget = (waypoint - pos).normalized * remainingDistance + pos; // When within [slowdownDistance] units from the target, use a progressively lower speed var desiredSpeed = Mathf.Clamp01(remainingDistance / slowdownDistance) * maxSpeed; Debug.DrawLine(transform.position, waypoint, Color.red); controller.SetTarget(rvoTarget, desiredSpeed, maxSpeed); } else { // Stand still controller.SetTarget(pos, maxSpeed, maxSpeed); } // Get a processed movement delta from the rvo controller and move the character. // This is based on information from earlier frames. var movementDelta = controller.CalculateMovementDelta(Time.deltaTime); pos += movementDelta; // Rotate the character if the velocity is not extremely small if (Time.deltaTime > 0 && movementDelta.magnitude / Time.deltaTime > 0.01f) { var rot = transform.rotation; var targetRot = Quaternion.LookRotation(movementDelta, controller.To3D(Vector2.zero, 1)); const float RotationSpeed = 5; if (controller.movementPlaneMode == MovementPlane.XY) { targetRot = targetRot * Quaternion.Euler(-90, 180, 0); } transform.rotation = Quaternion.Slerp(rot, targetRot, Time.deltaTime * RotationSpeed); } if (controller.movementPlaneMode == MovementPlane.XZ) { RaycastHit hit; if (Physics.Raycast(pos + Vector3.up, Vector3.down, out hit, 2, groundMask)) { pos.y = hit.point.y; } } transform.position = pos; }
public void SimplifyContour(List <int> verts, List <int> simplified, float maxError, int maxEdgeLenght, int buildFlags) { // Add initial points. bool hasConnections = false; for (int i = 0; i < verts.Count; i += 4) { if ((verts[i + 3] & ContourRegMask) != 0) { hasConnections = true; break; } } if (hasConnections) { // The contour has some portals to other regions. // Add a new point to every location where the region changes. for (int i = 0, ni = verts.Count / 4; i < ni; i++) { int ii = (i + 1) % ni; bool differentRegs = (verts[i * 4 + 3] & ContourRegMask) != (verts[ii * 4 + 3] & ContourRegMask); bool areaBorders = (verts[i * 4 + 3] & RC_AREA_BORDER) != (verts[ii * 4 + 3] & RC_AREA_BORDER); if (differentRegs || areaBorders) { simplified.Add(verts[i * 4 + 0]); simplified.Add(verts[i * 4 + 1]); simplified.Add(verts[i * 4 + 2]); simplified.Add(i); } } } if (simplified.Count == 0) { // If there is no connections at all, // create some initial points for the simplification process. // Find lower-left and upper-right vertices of the contour. int llx = verts[0]; int lly = verts[1]; int llz = verts[2]; int lli = 0; int urx = verts[0]; int ury = verts[1]; int urz = verts[2]; int uri = 0; for (int i = 0; i < verts.Count; i += 4) { int x = verts[i + 0]; int y = verts[i + 1]; int z = verts[i + 2]; if (x < llx || (x == llx && z < llz)) { llx = x; lly = y; llz = z; lli = i / 4; } if (x > urx || (x == urx && z > urz)) { urx = x; ury = y; urz = z; uri = i / 4; } } simplified.Add(llx); simplified.Add(lly); simplified.Add(llz); simplified.Add(lli); simplified.Add(urx); simplified.Add(ury); simplified.Add(urz); simplified.Add(uri); } // Add points until all raw points are within // error tolerance to the simplified shape. // This uses the Douglas-Peucker algorithm. int pn = verts.Count / 4; //Use the max squared error instead maxError *= maxError; for (int i = 0; i < simplified.Count / 4;) { int ii = (i + 1) % (simplified.Count / 4); int ax = simplified[i * 4 + 0]; int az = simplified[i * 4 + 2]; int ai = simplified[i * 4 + 3]; int bx = simplified[ii * 4 + 0]; int bz = simplified[ii * 4 + 2]; int bi = simplified[ii * 4 + 3]; // Find maximum deviation from the segment. float maxd = 0; int maxi = -1; int ci, cinc, endi; // Traverse the segment in lexilogical order so that the // max deviation is calculated similarly when traversing // opposite segments. if (bx > ax || (bx == ax && bz > az)) { cinc = 1; ci = (ai + cinc) % pn; endi = bi; } else { cinc = pn - 1; ci = (bi + cinc) % pn; endi = ai; Util.Memory.Swap(ref ax, ref bx); Util.Memory.Swap(ref az, ref bz); } // Tessellate only outer edges or edges between areas. if ((verts[ci * 4 + 3] & ContourRegMask) == 0 || (verts[ci * 4 + 3] & RC_AREA_BORDER) == RC_AREA_BORDER) { while (ci != endi) { float d2 = VectorMath.SqrDistancePointSegmentApproximate(verts[ci * 4 + 0], verts[ci * 4 + 2] / voxelArea.width, ax, az / voxelArea.width, bx, bz / voxelArea.width); if (d2 > maxd) { maxd = d2; maxi = ci; } ci = (ci + cinc) % pn; } } // If the max deviation is larger than accepted error, // add new point, else continue to next segment. if (maxi != -1 && maxd > maxError) { // Add space for the new point. //simplified.resize(simplified.size()+4); simplified.Add(0); simplified.Add(0); simplified.Add(0); simplified.Add(0); int n = simplified.Count / 4; for (int j = n - 1; j > i; --j) { simplified[j * 4 + 0] = simplified[(j - 1) * 4 + 0]; simplified[j * 4 + 1] = simplified[(j - 1) * 4 + 1]; simplified[j * 4 + 2] = simplified[(j - 1) * 4 + 2]; simplified[j * 4 + 3] = simplified[(j - 1) * 4 + 3]; } // Add the point. simplified[(i + 1) * 4 + 0] = verts[maxi * 4 + 0]; simplified[(i + 1) * 4 + 1] = verts[maxi * 4 + 1]; simplified[(i + 1) * 4 + 2] = verts[maxi * 4 + 2]; simplified[(i + 1) * 4 + 3] = maxi; } else { i++; } } //Split too long edges float maxEdgeLen = maxEdgeLength / cellSize; if (maxEdgeLen > 0 && (buildFlags & (RC_CONTOUR_TESS_WALL_EDGES | RC_CONTOUR_TESS_AREA_EDGES | RC_CONTOUR_TESS_TILE_EDGES)) != 0) { for (int i = 0; i < simplified.Count / 4;) { if (simplified.Count / 4 > 200) { break; } int ii = (i + 1) % (simplified.Count / 4); int ax = simplified[i * 4 + 0]; int az = simplified[i * 4 + 2]; int ai = simplified[i * 4 + 3]; int bx = simplified[ii * 4 + 0]; int bz = simplified[ii * 4 + 2]; int bi = simplified[ii * 4 + 3]; // Find maximum deviation from the segment. int maxi = -1; int ci = (ai + 1) % pn; // Tessellate only outer edges or edges between areas. bool tess = false; // Wall edges. if ((buildFlags & RC_CONTOUR_TESS_WALL_EDGES) != 0 && (verts[ci * 4 + 3] & ContourRegMask) == 0) { tess = true; } // Edges between areas. if ((buildFlags & RC_CONTOUR_TESS_AREA_EDGES) != 0 && (verts[ci * 4 + 3] & RC_AREA_BORDER) == RC_AREA_BORDER) { tess = true; } // Border of tile if ((buildFlags & RC_CONTOUR_TESS_TILE_EDGES) != 0 && (verts[ci * 4 + 3] & BorderReg) == BorderReg) { tess = true; } if (tess) { int dx = bx - ax; int dz = (bz / voxelArea.width) - (az / voxelArea.width); if (dx * dx + dz * dz > maxEdgeLen * maxEdgeLen) { // Round based on the segments in lexilogical order so that the // max tesselation is consistent regardles in which direction // segments are traversed. int n = bi < ai ? (bi + pn - ai) : (bi - ai); if (n > 1) { if (bx > ax || (bx == ax && bz > az)) { maxi = (ai + n / 2) % pn; } else { maxi = (ai + (n + 1) / 2) % pn; } } } } // If the max deviation is larger than accepted error, // add new point, else continue to next segment. if (maxi != -1) { // Add space for the new point. //simplified.resize(simplified.size()+4); simplified.AddRange(new int[4]); int n = simplified.Count / 4; for (int j = n - 1; j > i; --j) { simplified[j * 4 + 0] = simplified[(j - 1) * 4 + 0]; simplified[j * 4 + 1] = simplified[(j - 1) * 4 + 1]; simplified[j * 4 + 2] = simplified[(j - 1) * 4 + 2]; simplified[j * 4 + 3] = simplified[(j - 1) * 4 + 3]; } // Add the point. simplified[(i + 1) * 4 + 0] = verts[maxi * 4 + 0]; simplified[(i + 1) * 4 + 1] = verts[maxi * 4 + 1]; simplified[(i + 1) * 4 + 2] = verts[maxi * 4 + 2]; simplified[(i + 1) * 4 + 3] = maxi; } else { ++i; } } } for (int i = 0; i < simplified.Count / 4; i++) { // The edge vertex flag is take from the current raw point, // and the neighbour region is take from the next raw point. int ai = (simplified[i * 4 + 3] + 1) % pn; int bi = simplified[i * 4 + 3]; simplified[i * 4 + 3] = (verts[ai * 4 + 3] & ContourRegMask) | (verts[bi * 4 + 3] & RC_BORDER_VERTEX); } }
private void DelaunayRefinement(Int3[] verts, int[] tris, ref int tCount, bool delaunay, bool colinear) { if (tCount % 3 != 0) { throw new ArgumentException("Triangle array length must be a multiple of 3"); } Dictionary <Int2, int> dictionary = this.cached_Int2_int_dict; dictionary.Clear(); for (int i = 0; i < tCount; i += 3) { if (!VectorMath.IsClockwiseXZ(verts[tris[i]], verts[tris[i + 1]], verts[tris[i + 2]])) { int num = tris[i]; tris[i] = tris[i + 2]; tris[i + 2] = num; } dictionary[new Int2(tris[i], tris[i + 1])] = i + 2; dictionary[new Int2(tris[i + 1], tris[i + 2])] = i; dictionary[new Int2(tris[i + 2], tris[i])] = i + 1; } for (int j = 0; j < tCount; j += 3) { for (int k = 0; k < 3; k++) { int num2; if (dictionary.TryGetValue(new Int2(tris[j + (k + 1) % 3], tris[j + k % 3]), out num2)) { Int3 @int = verts[tris[j + (k + 2) % 3]]; Int3 int2 = verts[tris[j + (k + 1) % 3]]; Int3 int3 = verts[tris[j + (k + 3) % 3]]; Int3 int4 = verts[tris[num2]]; @int.y = 0; int2.y = 0; int3.y = 0; int4.y = 0; bool flag = false; if (!VectorMath.RightOrColinearXZ(@int, int3, int4) || VectorMath.RightXZ(@int, int2, int4)) { if (!colinear) { goto IL_3C6; } flag = true; } if (colinear && VectorMath.SqrDistancePointSegmentApproximate(@int, int4, int2) < 9f && !dictionary.ContainsKey(new Int2(tris[j + (k + 2) % 3], tris[j + (k + 1) % 3])) && !dictionary.ContainsKey(new Int2(tris[j + (k + 1) % 3], tris[num2]))) { tCount -= 3; int num3 = num2 / 3 * 3; tris[j + (k + 1) % 3] = tris[num2]; if (num3 != tCount) { tris[num3] = tris[tCount]; tris[num3 + 1] = tris[tCount + 1]; tris[num3 + 2] = tris[tCount + 2]; dictionary[new Int2(tris[num3], tris[num3 + 1])] = num3 + 2; dictionary[new Int2(tris[num3 + 1], tris[num3 + 2])] = num3; dictionary[new Int2(tris[num3 + 2], tris[num3])] = num3 + 1; tris[tCount] = 0; tris[tCount + 1] = 0; tris[tCount + 2] = 0; } else { tCount += 3; } dictionary[new Int2(tris[j], tris[j + 1])] = j + 2; dictionary[new Int2(tris[j + 1], tris[j + 2])] = j; dictionary[new Int2(tris[j + 2], tris[j])] = j + 1; } else if (delaunay && !flag) { float num4 = Int3.Angle(int2 - @int, int3 - @int); if (Int3.Angle(int2 - int4, int3 - int4) > 6.28318548f - 2f * num4) { tris[j + (k + 1) % 3] = tris[num2]; int num5 = num2 / 3 * 3; int num6 = num2 - num5; tris[num5 + (num6 - 1 + 3) % 3] = tris[j + (k + 2) % 3]; dictionary[new Int2(tris[j], tris[j + 1])] = j + 2; dictionary[new Int2(tris[j + 1], tris[j + 2])] = j; dictionary[new Int2(tris[j + 2], tris[j])] = j + 1; dictionary[new Int2(tris[num5], tris[num5 + 1])] = num5 + 2; dictionary[new Int2(tris[num5 + 1], tris[num5 + 2])] = num5; dictionary[new Int2(tris[num5 + 2], tris[num5])] = num5 + 1; } } } IL_3C6 :; } } }