Disabled some debugging.
Not checking for 'room' property when finding ground.

Just be careful when putting objects inside each other.
Not adding duplicate vertices to KD tree.
Casting lots of rays to determine relationships!