I have a function that spawns 4 objects in a row and those objects move down.I want to keep spawning 4 objects on top of previous 4 objects, but I want to avoid having empty horizontal space between those rows. Think of it like a chess board or any board with squared scrolling downwards indefinitely, and the 4 objects that I keep spawning are chess board squares.
This is my current function that spawns 4 objects in horizontal and I have a coroutine that calls this in random intervals
public void CreateLetter()
{
var wordLetters = randomChosenWord.ToCharArray();
for (int i = 0; i < letterPlacements.Length; i++)
{
var chosenLetter = wordLetters[Random.Range(0, wordLetters.Length)];
//Do not spawn too much duplicates
var numberOfSameLetters = FindObjectsOfType<LetterController>().Where(l => l.Letter == chosenLetter).ToList().Count;
var lettersAlreadyChosen = FindObjectOfType<PlayerControls>().playerChosenLetters;
//Did player choose any letter already? spawn that letter less
if (numberOfSameLetters > (lettersAlreadyChosen.IndexOf(chosenLetter) != -1 ? 1 : 2))
{
var trimmedRandomWord = randomChosenWord.Trim(chosenLetter);
chosenLetter = trimmedRandomWord[Random.Range(0, trimmedRandomWord.Length)];
}
GameObject letter = Instantiate(letterPrefab);
letter.transform.position = new Vector2(letterPlacements[i].transform.position.x, letterPlacements[i].transform.position.y + 1);
letter.GetComponent<SpriteRenderer>().sprite = letterSprites[alphabet.IndexOf(char.ToUpper(chosenLetter))];
letter.GetComponent<LetterController>().Letter = chosenLetter;
}
}
Should make a check if the new four square positions would collide with previous four? Any ideas or suggestions?
After some digging and calculating, I found out that what I need actually is called
inverse proportion.
To make the coroutine spawn 4 squares after the previous 4 have spawned (so that they are perfectly aligned), I use the movement speed of squares and then calculate the time when the coroutine should run again and spawn another 4 squares. In other words, the bigger the movement speed, the faster the coroutine function repeats:
IEnumerator GenerateLetters()
{
while(randomChosenWord.Length > 0)
{
yield return new WaitForSeconds(1 / (movementSpeed * 0.5f));
CreateLetter();
}
}
and this is the Update function in script attached to square prefab object:
// Update is called once per frame
void Update()
{
//Move letter downwards
transform.position = new Vector2(transform.position.x, transform.position.y - movementSpeed * Time.deltaTime / 2);
gameObject.GetComponentInChildren<TextMesh>().text = Letter.ToString();
}
I am trying to make a building game, where you can build anywhere that there is no builds and touching the ground. Here is my code
private void Update()
{
if (buildMode)
{
playerScript.enabled = false;
Vector3 point = Camera.main.ScreenToWorldPoint(Input.mousePosition);
if (Input.GetMouseButtonDown(0))
{
buildOverlay.ClearAllTiles();
Vector3Int selectedTile = buildOverlay.WorldToCell(point);
selectedTile.z = 0;
if (Vector3.Distance(GameObject.Find("Player").transform.position, selectedTile) < buildDistance)
{
//BoundsInt bounds = collidableTilemap.cellBounds;
//TileBase[] allTiles = collidableTilemap.GetTilesBlock(bounds);
//for (int x = 0; x < bounds.size.x; x++)
//{
// for (int y = 0; y < bounds.size.y; y++)
// {
// TileBase tile = allTiles[x + y * bounds.size.x];
// Debug.Log(collidableTilemap.HasTile(new Vector3Int(x, y, 0)));
// if (collidableTilemap.HasTile(new Vector3Int(x, y, 0)))
// {
// buildOverlay.SetTile(selectedTile, notplaceable);
// }
// else
// {
// buildOverlay.SetTile(selectedTile, placeable);
// }
// }
//}
selectedTile.z = 0;
Debug.Log(selectedTile);
Debug.Log(collidableTilemap.HasTile(selectedTile));
if (!collidableTilemap.HasTile(selectedTile))
{
buildOverlay.SetTile(selectedTile, placeable);
}
else
{
buildOverlay.SetTile(selectedTile, notplaceable);
}
}
}
else
{
playerScript.enabled = true;
}
}
}
What this code does so far is turn off the ability to move and check for an overlapping tile. I currently have the variable buildDistance set to infinity, so thats not the problem.
Here are some images:
Unity thinks that the first layer is not there. Here is the scene view to prove that those blocks are in the same tilemap:
This code is supposed to see if a tile exists in the coordinates where the player wants to place. It works fine except for the first layer. Please help!
I'm pretty new to Unity so don't mind my noob mistakes. Thanks!
[EDIT]: I've changed my terrain a bit, and realized a couple new things:
This block is red
This block is green.
I can not build anywhere on this row, except for when the stone ends:
I can build here:
WHAT IS GOING ON!!!!!????!!???
It's hard to say for certain but It seems that the conversion from world space to grid coordinate is off-by-one at least in the y dimension and i would guess in the x direction also.
I believe the best candidate for the bug is this line here
Vector3Int selectedTile = buildOverlay.WorldToCell(point);
This kind of casting from float to int actually won't round the number but will instead floor it. Unity often places it's tilemaps so tiles are 0.5m misaligned with the world grid and because of this flooring the position might be causing these problems.
I would suggest trying
Vector3Int selectedTile = buildOverlay.WorldToCell(Vector3Int.RoundToInt(point))
or if that does not help you could try the uglier
Vector3Int selectedTile = buildOverlay.WorldToCell(point+Vector3.one*0.5f);
(if this still doesn't work you could ommit the 0.5f)
Not the prettiest of solutions but I think this is where your problem is I'd have a play about with it.
So basically it is 1 unit off of the y axis, so simply subtract 1 y unit.
Replace this line:
if (!collidableTilemap.HasTile(selectedTile)
with
if (!collidableTilemap.HasTile(selectedTile - new Vector3Int(0, 1, 0)))
This will basically negate the offset effect that Unity puts on tilemaps from flooring.
I am new to Unity as well as C#. Yet, I am trying to make a simple 2D platform game where I made a prefab of an object called Block. What I want to be able to do is to create an array tile map with 0s and 1s where the 1s are blocks and 0s are nothing. Also, I don't want the tile map to be random. I want the blocks to be instantiated from another object called GameController. A perfect example of what I would like to achieve is something like this.
But I don't really know how to do this with an array. I want to keep things simple since I am trying to learn how unity and c# work. Any help would be appreciated.
So you can use some assets from the asset store( for example: https://www.assetstore.unity3d.com/en/#!/list/2965-procedural-generation) This is a pretty hard challange. I would recomand this video: https://www.youtube.com/watch?v=k1pWpYEt2UE , but the closest one to what you want to achieve is this one: https://www.youtube.com/watch?v=gIUVRYViG_g
Hope it helped.
You can make a 2 dimensional array, e.g. int[40, 100] and loop through it twice, and if the number in the array is one, multiply your position in the array by the length or width of your block respectively. For example:
int[,] positions = new int[40,100];
for (int i = 0; i < 41; i++) {
for (int j = 0; j < 100; j++) {
if (positions[i,j] = 1) {
GameObject temp = Instantiate(block, new Vector3(j * blockWidth, i * blockHeight, 0), Quaternion.identity) as GameObject;
}
}
}
It would take a long while to set all of the coordinates for an array this large, but you could loop through with parameters, or just do it the hard way if it is smaller. Otherwise, I would just try doing it without a script.
Okay so, I have looked at a lot of question from people like my self who are beginners in programming. And most of the time their questions are hard to answer or understand what the question really is. I will do my best to be as specific as possible about my problems so the above mentioned doesn't happen.
First off I have been following the tutorials over at http://xnagpa.net/xna4rpg.php. And my tile engine is based off the one that Jamie McMahon makes in his rpg tutorial series. Just so you know what the general structure of my tile engine is like.
Secondly I will try to explain what I'm trying to do inside the tile engine. I recently found an article about how the original Civilization generated their maps. http://forums.civfanatics.com/showthread.php?t=498630
And I rather like this approach to generating a "world" style map if you will. ie: oceans, continents, islands ect. So I decided to try to take this and implement it into my tile engine. It works for the most part. The parts that I added to the tile engine are supposed to randomly pick a location in the specified map layer (y,x) and then from that location generate a chunk of land(or tiles) and then replace the tiles in the map layer with the tiles created in the chunk of land. (ill show the code in a minute) and then do that for a desired amount of either number of landmasses(chunks) or continue creating chunks of land until the number of land tiles is equal to a desired amount of land tiles.
My Problem:
My program does what its supposed to (as mentioned above) except it only ever makes one landmass.(Chunk of land tiles) It does everything else just fine but it for some reason will not make more than one landmass. Now I suspect that it actually is making the other landmasses but somehow the way the tile engine is set up to display map layers is causing the landmass's to be covered up with water. Maybe its a layering issue. But It shouldn't be because the landmass's are all part of the same layer. So I'm completely baffled as to why its doing this.
public void GenerateLandChunks(MapLayer layer)
{
Tile tile = new Tile(0, 3);
Random random = new Random();
int x = random.Next(8, layer.Width - 10);
int y = random.Next(10, layer.Height - 20);
int length = random.Next(10, 70);
for (int i = 0; i < length; i++)
{
if (length != 0 && x > 8 || x < layer.Width - 10 && y > 10 || y < layer.Height - 20)
{
layer.SetTile(y, x, tile);
layer.SetTile(y, x + 1, tile);
layer.SetTile(y + 1, x, tile);
}
x = random.Next(x - 1, x + 2);
y = random.Next(y - 1, y + 2);
}
}
This is my method for generating the actual chunks it does what I want it to. (ABOVE)
MapLayer randomLayer = new MapLayer(100, 100);
for (int y = 0; y < randomLayer.Height; y++)
{
for (int x = 0; x < randomLayer.Width; x++)
{
Tile tile = new Tile(1, 3);
randomLayer.SetTile(x, y, tile);
}
}
int landMasses = 5;
for (int i = 0; i < landMasses; i++)
{
randomLayer.GenerateLandChunks(randomLayer);
}
This is where I create the map layer. I initially set the entire map to water tiles(tile (1,3)) then I tell it to generate 5 landmasses.
It seems like this should work. And it does but like I said only for the first one. It doesn't display the other 4 land masses.
My Question:
Is there anything you can see here that I'm doing wrong in order to accomplish what I'm trying to do?
If you need more of the code to understand whats going on let me know and ill post what ever you need. And like I said everything other than what I have posted is the exact same as it is in Jamie McMahon's tile engine.
I'm sorry if I have come off as unclear or if my question is hard to answer. I tried to make it as straight forward as possible. Thank you for your time.
So much text for such a simple answer. The problem is that a new Random object is generated every time, and so the "random" values are the same every time. That is how random number generators work, the numbers are not actually random, but "pseudorandom", and if you use the same random function and the same seed you will get the same progression of seemingly random numbers.
Default Random constructor seeds the generator with some value based on time, but if two generators are created with very small time period then that time value will be the same.
Move the Random object outside the function, so that it is used for all random generations, or move the loop inside (but Random outside the loop).
I've simplified things down to cubes/a single cube colliding with an infinite-mass rectangle and the following code:
The problem is, the boxes tend to spin too much and get stuck together spinning and, if the binary search is included, just hit and spin a lot.
Thanks for any and all help.
/// <summary>
/// Projects an abstract 1D line "perpendicular" to the axis,
/// stretching across the width of the model,
/// measured from that axis.
/// </summary>
/// <param name="Axis"></param>
/// <param name="Min"></param>
/// <param name="Max"></param>
protected virtual void ProjectToAxis(Vector2 Axis, IMotionData motionData, out double Min, out double Max)
{
Double DotP = Axis.Dot(motionData.PositionGS + (this.Vertices[0].Position * this.Model.Scale).Rotate(motionData.RotationGS));
Min = Max = DotP;
for (int t = 1; t < this.Vertices.Count(); ++t)
{
DotP = Axis.Dot(motionData.PositionGS + (this.Vertices[t].Position * this.Model.Scale).Rotate(motionData.RotationGS));
Min = Math.Min(DotP, Min);
Max = Math.Max(DotP, Max);
}
}
/// <summary>
/// Projects two imaginary lines even with each edge,
/// equal to the width of each object while looking at
/// that edge, then checks to see if they intersect.
/// </summary>
/// <param name="B1"></param>
/// <param name="B2"></param>
/// <returns></returns>
public static bool DetectCollision(Body B1, Body B2, Double elapsedSeconds)
{
CollisionData collisionInfo = new CollisionData();
double lowestDistance = double.MaxValue;
double distance;
Vector2 normalB1ToB2 = (B2.MotionHandler.PositionGS - B1.MotionHandler.PositionGS).Normalized;
foreach (Edge edge in B1.Edges)
{
if (edge.Normal.RelativePosition.Dot(normalB1ToB2) >= 0.0)
{
double minA, minB, maxA, maxB;
B1.ProjectToAxis(edge.Normal.RelativePosition, B1.MotionHandler.MotionDataGet, out minA, out maxA);
B2.ProjectToAxis(edge.Normal.RelativePosition, B2.MotionHandler.MotionDataGet, out minB, out maxB);
if (minA < minB)
distance = minB - maxA;
else
distance = minA - maxB;
if (distance > 0.0f)
return false;
else if (Math.Abs(distance) < lowestDistance)
{
lowestDistance = Math.Abs(distance);
collisionInfo.Normal = edge.Normal.RelativePosition;
collisionInfo.Edge = edge;
}
}
}
Vector2 normalB2ToB1 = -normalB1ToB2;
foreach (Edge edge in B2.Edges)
{
if (edge.Normal.RelativePosition.Dot(normalB2ToB1) >= 0.0)
{
double minA, minB, maxA, maxB;
B1.ProjectToAxis(edge.Normal.RelativePosition, B1.MotionHandler.MotionDataGet, out minA, out maxA);
B2.ProjectToAxis(edge.Normal.RelativePosition, B2.MotionHandler.MotionDataGet, out minB, out maxB);
if (minA < minB)
distance = minB - maxA;
else
distance = minA - maxB;
if (distance > 0.0f)
return false;
else if (Math.Abs(distance) < lowestDistance)
{
lowestDistance = Math.Abs(distance);
collisionInfo.Normal = edge.Normal.RelativePosition;
collisionInfo.Edge = edge;
}
}
}
collisionInfo.Depth = lowestDistance;
/* Double lowHighSeconds = elapsedSeconds;
Double highLowSeconds = 0.0;
Double seconds;
IMotionData md1;
IMotionData md2;
bool collision;
do
{
md1 = B1.MotionHandler.MotionDataLastGet.Copy;
md2 = B2.MotionHandler.MotionDataLastGet.Copy;
collision = true;
lowestDistance = Double.MaxValue;
seconds = MathExtensions.MathExt.Lerp(highLowSeconds, lowHighSeconds, 0.5);
B1.MotionHandler.Simulate(seconds, ref md1);
B2.MotionHandler.Simulate(seconds, ref md2);
normalB1ToB2 = (md2.PositionGS - md1.PositionGS).Normalized;
foreach (Edge edge in B1.Edges)
{
if ((edge.Normal.Position * B1.Model.Scale).Rotate(md1.RotationGS).Dot(normalB1ToB2) >= 0.0)
{
double minA, minB, maxA, maxB;
B1.ProjectToAxis((edge.Normal.Position * B1.Model.Scale).Rotate(md1.RotationGS), md1, out minA, out maxA);
B2.ProjectToAxis((edge.Normal.Position * B1.Model.Scale).Rotate(md1.RotationGS), md2, out minB, out maxB);
if (minA < minB)
distance = minB - maxA;
else
distance = minA - maxB;
if (distance > 0.0f)
collision = false;
else if (Math.Abs(distance) < lowestDistance)
{
lowestDistance = Math.Abs(distance);
collisionInfo.Normal = (edge.Normal.Position * B1.Model.Scale).Rotate(md1.RotationGS);
collisionInfo.Edge = edge;
}
}
}
normalB2ToB1 = -normalB1ToB2;
foreach (Edge edge in B2.Edges)
{
if ((edge.Normal.Position * B2.Model.Scale).Rotate(md2.RotationGS).Dot(normalB2ToB1) >= 0.0)
{
double minA, minB, maxA, maxB;
B2.ProjectToAxis((edge.Normal.Position * B2.Model.Scale).Rotate(md2.RotationGS), md2, out minA, out maxA);
B1.ProjectToAxis((edge.Normal.Position * B2.Model.Scale).Rotate(md2.RotationGS), md1, out minB, out maxB);
if (minA < minB)
distance = minB - maxA;
else
distance = minA - maxB;
if (distance > 0.0f)
collision = false;
else if (Math.Abs(distance) < lowestDistance)
{
lowestDistance = Math.Abs(distance);
collisionInfo.Normal = (edge.Normal.Position * B2.Model.Scale).Rotate(md2.RotationGS);
collisionInfo.Edge = edge;
}
}
}
collisionInfo.Depth = lowestDistance;
if (!collision)
{
lowHighSeconds = seconds;
}
else
{
highLowSeconds = seconds;
}
} while (Math.Abs(highLowSeconds - lowHighSeconds) > 0.0001);
B1.MotionHandler.MotionDataSet = md1;
B2.MotionHandler.MotionDataSet = md2; */
// bool flip = false;
if (collisionInfo.Edge.Parent != B2.Model)
{
Body temp = B1;
B1 = B2;
B2 = temp;
}
//This is needed to make sure that the collision normal is pointing at B1
int Sign = Math.Sign(
collisionInfo.Normal.Dot(
B1.MotionHandler.MotionDataGet.PositionGS + (B1.Center * B1.Model.Scale).Rotate(B1.MotionHandler.MotionDataGet.RotationGS) -
B2.MotionHandler.MotionDataGet.PositionGS + (B2.Center * B2.Model.Scale).Rotate(B2.MotionHandler.MotionDataGet.RotationGS)
)
);
//Remember that the line equation is N*( R - R0 ). We choose B2->Center
//as R0; the normal N is given by the collision normal
if (Sign != 1)
collisionInfo.Normal = -collisionInfo.Normal; //Revert the collision normal if it points away from B1
double SmallestD = double.MaxValue; //Initialize the smallest distance to a high value
//Measure the distance of the vertex from the line using the line equation
for (int t = 0; t < B1.Vertices.Count(); ++t)
{
double Distance = collisionInfo.Normal.Dot(B1.Vertices[t].WorldPosition - B2.Center);
// If the measured distance is smaller than the smallest distance reported
// so far, set the smallest distance and the collision vertex
if (Distance < SmallestD)
{
SmallestD = Distance;
collisionInfo.Vertex = B1.Vertices[t];
}
}
if ((Body.CollisionType & CollisionType.Velocity) > 0)
{
Vector2 vab1 = B1.MotionHandler.VelocityGS - B2.MotionHandler.VelocityGS;
Vector2 rap = (B1.MotionHandler.PositionGS - collisionInfo.Normal);
Vector2 rbp = (B2.MotionHandler.PositionGS - collisionInfo.Normal);
Double rap2 = (rap.Cross(collisionInfo.Normal));
Double rbp2 = (rbp.Cross(collisionInfo.Normal));
Vector2 one = (collisionInfo.Vertex.WorldPosition - B1.MotionHandler.PositionGS).GetPerpendicular;
Vector2 two = (collisionInfo.Vertex.WorldPosition - B2.MotionHandler.PositionGS).GetPerpendicular;
Double j = (-(1 + 0.0) * vab1.Dot(collisionInfo.Normal)) /
((collisionInfo.Normal.Dot(collisionInfo.Normal) * (B1.MotionHandler.InverseMassGS + B2.MotionHandler.InverseMassGS)) +
(one.Dot(one) * B1.MotionHandler.InverseInertiaGS) + (two.Dot(two) * B2.MotionHandler.InverseInertiaGS));
B1.MotionHandler.AddImpulse = new Force(
collisionInfo.Normal,
j /* ,
one */
);
B2.MotionHandler.AddImpulse = new Force(
collisionInfo.Normal,
-(j) /* ,
two */
);
NewtonianMotionData data1 = (NewtonianMotionData)B1.MotionHandler.MotionDataGet;
NewtonianMotionData data2 = (NewtonianMotionData)B2.MotionHandler.MotionDataGet;
data1.AngularVelocity += (one.Dot(j * collisionInfo.Normal)) * data1.inverseInertia;
data2.AngularVelocity += (two.Dot(-j * collisionInfo.Normal)) * data2.inverseInertia;
B1.MotionHandler.MotionDataSet = data1;
B2.MotionHandler.MotionDataSet = data2;
}
return true;
}
You've got two problems.
1) There's something wrong with the code. You need to fix that.
2) You don't know how to figure out what "something" is.
Solving the first problem is gated on your solving the second problem. You need to learn how to debug a program you just wrote.
You've already tested it and gotten a result which you've identified as nonsensical. That's a good first step. Now break it down even farther. Pick a simple problem in this domain that you can solve yourself with pencil and paper; do so, and then watch your algorithm solve the same problem in the debugger, checking every step along the way. Listen to quiet nagging doubts. When there is anything that looks slightly off or unexpected, stop what you're doing and investigate the issue until you understand whether things are working correctly or not. Eventually you'll find a step where things aren't as they should be, and that's where the bug is.
Yes, this is tedious. When you've found the bug and fixed it, pause and reflect upon what caused you to write the bug in the first place, and figure out how to not write that kind of bug ever again.
UPDATE:
Re: your recent comments.
Apology accepted. Now calm down. You're never going to find this bug if you're this worked up. Your brain will not let you. Humans who are in a panicky, worked-up state lose the ability to reason. That's why fire doors open outwards; humans fleeing a burning building literally will not stop to think "I'm pushing on this door and its not opening, maybe I should try pulling". They just push harder. I suspect you are pushing harder.
Debugging requires rationality and careful attention to small details. If you're all worked up about this problem then that's going to go out the window and its just going to get worse. Take it from someone who has been there. We've all been there. It's a deeply frustrating thing to have caused a bug in your own program that you then cannot find.
The reason no one is helping you is because... well, let me list the set of preconditions that have to be met for me to help you with more than vague platitudes and suggestions of how to focus your debugging efforts:
1) I have to know something about simulation of 3d physics. I had a pretty decent grasp of the differential equations of simple of Newtonian mechanics in 1992, but I haven't used it since. And the equation of a damped driven spring is rather different than the equations of rigid body collisions. If I spent a couple weeks reviewing my notes I could get the math back, but that's not realistic. You need someone who is deeply conversant right now with 3d collision physics simulations.
2) I have to be able to read and understand your code, code which is hundreds of lines long, written by someone other than me, to solve a problem I'm not familiar with. Worse, a hundred lines of that code is commented out. Why? Is it relevant? Is the bug in there? Moreover, I need to be able to read and understand the code without running it in a debugger. Heck, I can't even compile that code. It depends on libraries that I don't have.
And even worse still, one of those libraries might contain the bug. For all I know, the bug is a typo in some code that calculates a normal somewhere that you haven't shown us. The code shown could be perfect.
3) I need to have the free time to work on someone else's hard problem; a problem that the person who wrote the code and understands the physics is making no headway on.
All of these are requirements; if any one of them is missing, the reader cannot effectively help you. You're asking people you don't know to help you find a black cat in a dark warehouse at midnight without a flashlight -- a cat that might not even be there. It's not surprising you're getting few takers. Of the 74 stack overflow users who have read your question, how many of them meet all three requirements? I meet none of them.
If you want help on this site then post an easier problem. Narrow the problem down to a problem that requires less special knowledge of physics and simulation algorithms and has only the relevant code, preferably code that can be compiled and run.
This may not be good news, but I have a couple of things to add to Eric Lippert's analysis, and a suggestion.
Your comments are misleading. I know that if you're not familiar with math and physics it's hard to be precise, but take a look at "ProjectToAxis":
/// Projects an abstract 1D line "perpendicular" to the axis,
/// stretching across the width of the model,
/// measured from that axis.
Forgive me if this sounds harsh, but
"abstract 1d line" is kind of meaningless, it should just say "line".
It's not really projecting a line.
It's measuring extent parallel to the axis, not perpendicular to it.
It's not "across the width", exactly, it's just the greatest extent.
"measured from that axis" is either meaningless or wrong, I can't tell which.
Believe me, I'm not trying to pick nits, it's just that I'm trying to figure out what this code is supposed to do, and a bad comment is worse than none. I can see what this function does (assuming that functions like "Dot" work as advertised), but I still don't know whether it does what you want it to do.
Now I take a look at DetectCollision (which does more than just detect a collision):
/// Projects two imaginary lines even with each edge,
/// equal to the width of each object while looking at
/// that edge, then checks to see if they intersect.
What? All I can do is ignore this and look at the code... There are parts of it that don't make much sense (e.g. why the heck do you project a body onto every one of its edges?), so reverse-engineering is going to be very difficult.
If I knew the algorithm you were trying for, I could try to find the bug. If the code worked, I could try to deduce the algorithm. But if the code doesn't work and (as I suspect) you don't really know the algorithm yourself, we're kind of stuck.
Here's an approach that might work: This function is too long, it does a lot, and you don't know which parts it does correctly. Ergo you should break it down into several functions and test them individually. (I can't do that myself, for the reasons Eric Lippert spelled out.) You could start by breaking into two functions, one that calculates CollisionInfo (leaving the bodies constant) and another that adjusts the motion of the bodies (leaving CollisionInfo constant).