--- a/src/proto2/Physics.cc	Mon Nov 24 17:14:29 2008 +0000
+++ b/src/proto2/Physics.cc	Mon Nov 24 21:08:36 2008 +0000
@@ -174,8 +174,24 @@
 
 		float minVelocity = 10;
 		// Check if any of the four corners of the worm collide
-		/*if(velocity.y > 0) {
-        	if(world.getType(tmpVector+shape[2])) {
+		for(int sh = 0; sh < 4; sh++) {
+            if(world.getType(tmpVector+shape[2]) != EMPTY) {
+				reached = position + unitVector*(i-1);
+				collided = true;
+				this->bounce(world.getNormal(tmpVector+shape[2], tmpVector-unitVector+shape[2]));
+				this->velocity *= 0.6;
+				if(abs(this->velocity.x) < minVelocity && (abs(this->velocity.y) < minVelocity)) {
+					this->inAir = false;
+					this->velocity = Vector(0,0);
+				}
+				break;
+			}
+		}
+		if(collided)
+			break;
+		/*
+		if(velocity.y > 0) {
+        	if(world.getType(tmpVector+shape[2]) != EMPTY) {
             	reached = position + unitVector*(i-1);
             	collided = true;
 				this->velocity.y *= -0.3;
@@ -187,7 +203,7 @@
             	break;
 			}
 		} else {
-        	if(world.getType(tmpVector+shape[0])) {
+        	if(world.getType(tmpVector+shape[0]) != EMPTY) {
             	reached = position + unitVector*(i-1);
             	collided = true;
 				this->velocity.y *= -0.3;
@@ -201,7 +217,7 @@
         }
 
 		if(velocity.x > 0) {
-        	if(world.getType(tmpVector+shape[1])) {
+        	if(world.getType(tmpVector+shape[1]) != EMPTY) {
             	reached = position + unitVector*(i-1);
             	collided = true;
 				this->velocity.x *= -0.6;
@@ -213,7 +229,7 @@
             	break;
 			}
 		} else {
-        	if(world.getType(tmpVector+shape[3])) {
+        	if(world.getType(tmpVector+shape[3]) != EMPTY) {
             	reached = position + unitVector*(i-1);
             	collided = true;
 				this->velocity.x *= -0.6;
@@ -224,10 +240,10 @@
 				}
             	break;
 			}
-        }*/     
+        }*/
 		
 		// This col. det. doesn't let worms inside the ground, but on the other hand the worms get often stuck
-		if(world.getType(tmpVector+shape[0]) != EMPTY || (world.getType(tmpVector+shape[2]) != EMPTY)) {
+		/*if(world.getType(tmpVector+shape[0]) != EMPTY || (world.getType(tmpVector+shape[2]) != EMPTY)) {
             reached = position + unitVector*(i-1);
             collided = true;
 			this->velocity.y *= -0.3;
@@ -246,7 +262,7 @@
 				this->velocity = Vector(0,0);
 			}
             break;
-		}
+		}*/
 		
 			//Engine::log(DEBUG, "physics.update_position") << "didnt hit";
     }
@@ -270,18 +286,62 @@
     this->position = newPosition;
 
 }
-/*
+
+/**
+ * Gets the index of the given coordinate direction
+ * referring to the DIRECTIONS table in Physics.hh
+ */
 int getDirectionIndex (Vector dir) {
 	int index = 0;
 	if(dir.x < -0.1 || (dir.y > 0.1 && dir.x < 0.1))
 		index += 4;
-	if((dir.x > 0.1 && dir.y 
+	if((dir.x > 0.1 && dir.y > 0.1) || (dir.y > 0.1 && dir.x < 0.1)) {
+		index += 2;
+	if(!(dir.x > -0.1 && dir < 0.1) && !(dir.y < 0.1 && dir.y > -0.1)) {
+		index += 1;
+	return index;
 }
 
+/**
+ * Computes hitten wall's normal. Calculated from 3*3 grid
+ */
 Vector PhysicsWorld::getNormal (Vector hitPoint, Vector prevPoint) {
 	// Search free points with bfs and put them to vector
 	std::vector<Vector> frees;
-*/	
+	Vector hit = Vector((int)hitPoint.x, (int)hitPoint.y);
+	Vector prev = Vector((int)prevPoint.x, (int)prevPoint.y);
+	int dirIdx = getDirectionIndex(hit-prev);
+	for(int i = 1; i <= 2; i++) {
+		if(getType(hit+DIRECTIONS[dirIdx+i]) == EMPTY)
+			frees.push_back(DIRECTIONS[dirIdx+i]);
+		else
+			break;
+	}
+	for(int i = 1; i <= 2; i++) {
+		if(getType(hit+DIRECTIONS[dirIdx-i]) == EMPTY)
+			frees.push_back(DIRECTIONS[dirIdx-i]);
+		else
+			break;
+	}
+    frees.push_back(DIRECTION[dirIdx]);
+
+	Vector normal(0,0);
+	for(int i = 0; i < size(); i++) {
+		normal += frees[i];
+	}
+	return normal;
+}
+
+/**
+ * Bounces from straight wall in any direction.
+ * Direction given as normal of that wall
+ */
+void PhysicsObject::bounce (Vector normal) {
+    Vector tangent(normal.y, -normal.x);
+	Vector tprojection = velocity * tangent / tangent.length();
+	Vector nprojection = velocity * normal / normal.length();
+	velocity = tprojection - nprojection;
+}
 
 /*bool PhysicsWorld::collided (Vector oldPos, Vector newPos) {
 	return false;