public List <CollisionPointStructure> GetWarmStartedCollisionPoints( IShape[] shapes, List <CollisionPointStructure> previousCollisionPoints, List <CollisionPointStructure> actualCollisionPoints, Dictionary <int, StabilizationValues> previousShapesProperties) { UpdatePersistentCollisionCounter( previousCollisionPoints, actualCollisionPoints, previousShapesProperties, shapes); Dictionary <WarmStartHashSet, CollisionPointStructure> warmStartedCollisionPoints = new Dictionary <WarmStartHashSet, CollisionPointStructure>(); int warmStartCounter = 15; foreach (var item in actualCollisionPoints) { WarmStartHashSet hashset = new WarmStartHashSet( item.ObjectIndexA, item.ObjectIndexB); if (collisionPointsCounterDictionary.TryGetValue(hashset, out Tuple <bool, int, CollisionPointStructure> value) && value.Item2 >= warmStartCounter) { warmStartedCollisionPoints.Add(new WarmStartHashSet(item.ObjectIndexA, item.ObjectIndexB), value.Item3); //} } } var outputCollisionPoints = new List <CollisionPointStructure>(actualCollisionPoints); for (int i = 0; i < outputCollisionPoints.Count; i++) { var hashSet = new WarmStartHashSet(outputCollisionPoints[i].ObjectIndexA, outputCollisionPoints[i].ObjectIndexB); if (warmStartedCollisionPoints.TryGetValue(hashSet, out CollisionPointStructure cPoint)) { CollisionPointStructure item = outputCollisionPoints[i]; int k = 0; foreach (var value in item.CollisionPointBase[0].CollisionPoints) { //if (!value.Intersection) //{ List <StartImpulseProperties> startImpulse = new List <StartImpulseProperties> { new StartImpulseProperties(0.0), new StartImpulseProperties(0.0), new StartImpulseProperties(0.0) }; //CollisionPoint val = value; //Get nearest double minDistance = double.MaxValue; foreach (var point in cPoint.CollisionPointBase[0].CollisionPoints) { double distAA = Vector3d.Length(point.CollisionPointA.Vertex - value.CollisionPointA.Vertex); double distBA = Vector3d.Length(point.CollisionPointB.Vertex - value.CollisionPointA.Vertex); if (distAA < minDistance || distBA < minDistance) { minDistance = (distAA < distBA) ? distAA : distBA; //startImpulse = point.StartImpulseValue; } /* * if ( * (CheckCollisionPointDistance(point.CollisionPointA.Vertex, value.CollisionPointA.Vertex, 1E-2) && * CheckCollisionPointDistance(point.CollisionPointB.Vertex, value.CollisionPointB.Vertex, 1E-2)) || * (CheckCollisionPointDistance(point.CollisionPointA.Vertex, value.CollisionPointB.Vertex, 1E-2) && * CheckCollisionPointDistance(point.CollisionPointB.Vertex, value.CollisionPointA.Vertex, 1E-2))) * { * startImpulse = point.StartImpulseValue; * break; * } */ } k++; //} //value.SetNormal(outputCollisionPoints[i].CollisionPointBase[0].CollisionPoint.CollisionNormal); } outputCollisionPoints[i] = item; //} } } return(outputCollisionPoints); }
private void UpdatePersistentCollisionCounter( List <CollisionPointStructure> previousCollisionPoints, List <CollisionPointStructure> actualCollisionPoints, Dictionary <int, StabilizationValues> previousShapesProperties, IShape[] shapes) { double distTolerance = 0.001; double velTolerance = 0.2; if (actualCollisionPoints != null && actualCollisionPoints.Count > 0) { //Check persistent collision var pCounter = collisionPointsCounterDictionary.ToArray(); for (int i = 0; i < pCounter.Length; i++) { var tupleVal = new Tuple <bool, int, CollisionPointStructure>(false, pCounter[i].Value.Item2, pCounter[i].Value.Item3); pCounter[i] = new KeyValuePair <WarmStartHashSet, Tuple <bool, int, CollisionPointStructure> >(pCounter[i].Key, tupleVal); } collisionPointsCounterDictionary = pCounter.ToDictionary(x => x.Key, x => x.Value); for (int i = 0; i < actualCollisionPoints.Count; i++) { WarmStartHashSet hash = new WarmStartHashSet( actualCollisionPoints[i].ObjectIndexA, actualCollisionPoints[i].ObjectIndexB); if (collisionPointsCounterDictionary.TryGetValue(hash, out Tuple <bool, int, CollisionPointStructure> value)) { if (previousShapesProperties.TryGetValue(hash.ID_A, out StabilizationValues objA) && previousShapesProperties.TryGetValue(hash.ID_B, out StabilizationValues objB)) { //TODO Check position and velocity var previousPoint = previousCollisionPoints.FirstOrDefault(x => x.ObjectIndexA == hash.ID_A && x.ObjectIndexB == hash.ID_B); if (previousPoint == null) { previousPoint = previousCollisionPoints.FirstOrDefault(x => x.ObjectIndexA == hash.ID_B && x.ObjectIndexB == hash.ID_A); hash = new WarmStartHashSet( actualCollisionPoints[i].ObjectIndexB, actualCollisionPoints[i].ObjectIndexA); } /* * var shapeA = shapes.First(x => x.ID == hash.ID_A); * var shapeB = shapes.First(x => x.ID == hash.ID_B); * * var posDiffA = Vector3.Length(shapeA.Position - objA.Position); * var posDiffB = Vector3.Length(shapeB.Position - objB.Position); * * var angDiffA = Quaternion.Length(shapeA.RotationStatus - objA.RotationStatus); * var angDiffB = Quaternion.Length(shapeB.RotationStatus - objB.RotationStatus); * * * if (previousPoint.CollisionPointBase[0].CollisionPoints.Length < 10) * { * var cPoint = actualCollisionPoints[i].CollisionPointBase[0].CollisionPoint; * cPoint.SetNormal(previousPoint.CollisionPointBase[0].CollisionPoint.CollisionNormal); * previousPoint.CollisionPointBase[0].AddCollisionPoint(cPoint); * } */ collisionPointsCounterDictionary[hash] = new Tuple <bool, int, CollisionPointStructure>(true, value.Item2 + 1, previousPoint); /* * if (posDiffA < distTolerance && posDiffB < distTolerance && * angDiffA < distTolerance && angDiffB < distTolerance) * { * collisionPointsCounterDictionary[hash] = new Tuple<bool, int, CollisionPointStructure>(true, value.Item2 + 1, previousPoint); * } * else * { * if (collisionPointsCounterDictionary.ContainsKey(hash)) * collisionPointsCounterDictionary[hash] = new Tuple<bool, int, CollisionPointStructure>(true, 0, actualCollisionPoints[i]); * else * collisionPointsCounterDictionary.Add(hash, new Tuple<bool, int, CollisionPointStructure>(true, 0, actualCollisionPoints[i])); * } */ } } else { collisionPointsCounterDictionary.Add(hash, new Tuple <bool, int, CollisionPointStructure>(true, 0, actualCollisionPoints[i])); } } var itemToDelete = collisionPointsCounterDictionary.ToArray().Where(x => !x.Value.Item1); foreach (var item in itemToDelete) { collisionPointsCounterDictionary.Remove(item.Key); } } else { collisionPointsCounterDictionary = new Dictionary <WarmStartHashSet, Tuple <bool, int, CollisionPointStructure> >(); } }