openspades/Sources/Client/Corpse.cpp

/*
 Copyright (c) 2013 yvt
 
 This file is part of OpenSpades.
 
 OpenSpades is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 OpenSpades is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with OpenSpades.  If not, see <http://www.gnu.org/licenses/>.
 
 */

#include "Corpse.h"
#include "GameMap.h"
#include "IRenderer.h"
#include "Player.h"
#include "World.h"
#include "IModel.h"
#include "../Core/Debug.h"
#include "../Core/Settings.h"

SPADES_SETTING(r_corpseLineCollision, "1");

namespace spades {
	namespace client {
		Corpse::Corpse(IRenderer *renderer,
					   GameMap *map,
					   Player *p):
		renderer(renderer), map(map) {
			SPADES_MARK_FUNCTION();
			
			IntVector3 col = p->GetWorld()->GetTeam(p->GetTeamId()).color;
			color = MakeVector3(col.x / 255.f,
								col.y / 255.f,
								col.z / 255.f);
			
			bool crouch = p->GetInput().crouch;
			Vector3 front = p->GetFront();
			
			float yaw = atan2(front.y, front.x) + static_cast<float>(M_PI) * .5f;
			//float pitch = -atan2(front.z, sqrt(front.x * front.x + front.y * front.y));
			
			// lower axis
			Matrix4 lower = Matrix4::Translate(p->GetOrigin());
			lower = lower * Matrix4::Rotate(MakeVector3(0,0,1),
											yaw);
			
			Matrix4 torso;
			
			if(crouch){
				lower = lower * Matrix4::Translate(0, 0, -0.4f);
				torso = lower * Matrix4::Translate(0, 0, -0.3f);
				
				SetNode(Torso1,
						torso*MakeVector3(0.4f, -.15f, 0.1f));
				SetNode(Torso2,
						torso*MakeVector3(-0.4f, -.15f, 0.1f));
				SetNode(Torso3,
						torso*MakeVector3(-0.4f, .8f, 0.7f));
				SetNode(Torso4,
						torso*MakeVector3(0.4f, .8f, 0.7f));
				
				SetNode(Leg1,
						lower*MakeVector3(-0.4f, .1f, 1.f));
				SetNode(Leg2,
						lower*MakeVector3(0.4f, .1f, 1.f));
				
				SetNode(Arm1,
						torso*MakeVector3(0.2f, -.4f, .2f));
				SetNode(Arm2,
						torso*MakeVector3(-0.2f, -.4f, .2f));
				
			}else{
				torso = lower * Matrix4::Translate(0, 0, -1.1f);
				
				SetNode(Torso1,
						torso*MakeVector3(0.4f, 0.f, 0.1f));
				SetNode(Torso2,
						torso*MakeVector3(-0.4f, 0.f, 0.1f));
				SetNode(Torso3,
						torso*MakeVector3(-0.4f, .0f, 1.f));
				SetNode(Torso4,
						torso*MakeVector3(0.4f, .0f, 1.f));
				
				SetNode(Leg1,
						lower*MakeVector3(-0.4f, .0f, 1.f));
				SetNode(Leg2,
						lower*MakeVector3(0.4f, .0f, 1.f));
				
				SetNode(Arm1,
						torso*MakeVector3(0.2f, -.4f, .2f));
				SetNode(Arm2,
						torso*MakeVector3(-0.2f, -.4f, .2f));
				
			}
			
			SetNode(Head, (nodes[Torso1].pos + nodes[Torso2].pos)
					* .5f + MakeVector3(0,0,-0.6f));
			
			
		}
		
		static float VelNoise() {
			return (GetRandom() - GetRandom()) * 2.f;
		}
		
		void Corpse::SetNode(NodeType n, spades::Vector3 v){
			SPAssert(n >= 0); SPAssert(n < NodeCount);
			nodes[n].pos = v;
			nodes[n].vel = MakeVector3(VelNoise(),
									   VelNoise(),
									   0.f);
			nodes[n].lastPos = v;
			nodes[n].lastForce = MakeVector3(0, 0,0);
			
		}
		void Corpse::SetNode(NodeType n, spades::Vector4 v){
			SetNode(n, v.GetXYZ());
		}
		
		Corpse::~Corpse(){
			
		}
		
		void Corpse::Spring(NodeType n1,
							NodeType n2,
							float distance,
							float dt){
			SPADES_MARK_FUNCTION_DEBUG();
			SPAssert(n1 >= 0); SPAssert(n1 < NodeCount);
			SPAssert(n2 >= 0); SPAssert(n2 < NodeCount);
			Node& a = nodes[n1];
			Node& b = nodes[n2];
			Vector3 diff = b.pos - a.pos;
			float dist = diff.GetLength();
			Vector3 force = diff.Normalize() * (distance - dist);
			force *= dt * 50.f;
			
			b.vel += force;
			a.vel -= force;
			
			b.pos += force / (dt * 50.f) * 0.5f;
			a.pos -= force / (dt * 50.f) * 0.5f;
			
			Vector3 velMid = (a.vel + b.vel) * .5f;
			float dump = 1.f - powf(.1f, dt);
			a.vel += (velMid - a.vel) * dump;
			b.vel += (velMid - b.vel) * dump;
			
		}
		void Corpse::Spring(NodeType n1a,
							NodeType n1b,
							 NodeType n2,
							 float distance,
							 float dt){
			SPADES_MARK_FUNCTION_DEBUG();
			SPAssert(n1a >= 0); SPAssert(n1a < NodeCount);
			SPAssert(n1b >= 0); SPAssert(n1b < NodeCount);
			SPAssert(n2 >= 0); SPAssert(n2 < NodeCount);
			Node& x = nodes[n1a];
			Node& y = nodes[n1b];
			Node& b = nodes[n2];
			Vector3 diff = b.pos - (x.pos + y.pos) * .5f;
			float dist = diff.GetLength();
			Vector3 force = diff.Normalize() * (distance - dist);
			force *= dt * 50.f;
			
			b.vel += force;
			force *= .5f;
			x.vel -= force;
			y.vel -= force;
			
			Vector3 velMid = (x.vel + y.vel) * .25f + b.vel * .5f;
			float dump = 1.f - powf(.05f, dt);
			x.vel += (velMid - x.vel) * dump;
			y.vel += (velMid - y.vel) * dump;
			b.vel += (velMid - b.vel) * dump;
			
		}
		
		static float MyACos(float v){
			SPAssert(!isnan(v));
			if(v >= 1.f) return 0.f;
			if(v <= -1.f) return static_cast<float>(M_PI);
			float vv = acosf(v);
			if(isnan(vv)){
				vv = acosf(v * .9999f);
			}
			SPAssert(!isnan(vv));
			return vv;
		}

		void Corpse::AngleSpring(NodeType base,
								 NodeType n1id,
								 NodeType n2id,
								 float minDot,
								 float maxDot,
								 float dt){
			Node& nBase = nodes[base];
			Node& n1 = nodes[n1id];
			Node& n2 = nodes[n2id];
			Vector3 d1 = n1.pos - nBase.pos;
			Vector3 d2 = n2.pos - nBase.pos;
			float ln1 = d1.GetLength();
			float ln2 = d2.GetLength();
			float dot = Vector3::Dot(d1, d2) / (ln1 * ln2 + 0.0000001f);
			
			if(dot >= minDot && dot <= maxDot)
				return;
			
			Vector3 diff = n2.pos - n1.pos;
			float strength = 0.f;
			
			Vector3 a1 = Vector3::Cross(d1, diff);
			a1 = Vector3::Cross(d1, a1).Normalize();
			
			Vector3 a2 = Vector3::Cross(d2, diff);
			a2 = Vector3::Cross(d2, a2).Normalize();
			
			//a1=-a1; a2=-a2;
			//a1 = -a1;
			a2 = -a2;
			
			if(dot > maxDot){
				strength = MyACos(dot) - MyACos(maxDot);
			}else if(dot < minDot){
				strength = MyACos(dot) - MyACos(minDot);
			}
			
			SPAssert(!isnan(strength));
			
			strength *= 20.f;
			strength *= dt;
			
			a1 *= strength;
			a2 *= strength;
			
			a2 *= 0.f;
			
			n2.vel += a1;
			n1.vel += a2;
			nBase.vel -= a1 + a2;
			
			/*
			d1 += a1 * 0.01;
			d2 += a2 * 0.01;
			float nd = Vector3::Dot(d1, d2) / (d1.GetLength() * d2.GetLength());
			
			if(dot > maxDot){
				if(nd < dot)
					printf("GOOD %f -> %f\n", dot, nd);
				else
					printf("BAD %f -> %f\n", dot, nd);
			}else{
				if(nd > dot)
					printf("GOOD %f -> %f\n", dot, nd);
				else
					printf("BAD %f -> %f\n", dot, nd);
			}*/
		}
		void Corpse::AngleSpring(NodeType n1id,
								 NodeType n2id,
								 Vector3 dir,
								 float minDot,
								 float maxDot,
								 float dt){
			Node& n1 = nodes[n1id];
			Node& n2 = nodes[n2id];
			Vector3 diff = n2.pos - n1.pos;
			float ln1 = diff.GetLength();
			float dot = Vector3::Dot(diff, dir) / (ln1 + 0.000000001f);
			
			if(dot >= minDot && dot <= maxDot)
				return;
			
			float strength = 0.f;
			
			Vector3 a1 = Vector3::Cross(dir, diff);
			a1 = Vector3::Cross(diff, a1).Normalize();
			
			Vector3 a2 = -a1;
			//a1=-a1; a2=-a2;
			//a1 = -a1;
			
			if(dot > maxDot){
				strength = MyACos(dot) - MyACos(maxDot);
			}else if(dot < minDot){
				strength = MyACos(dot) - MyACos(minDot);
			}
			
			SPAssert(!isnan(strength));
			
			strength *= 100.f;
			strength *= dt;
			
			a1 *= strength;
			a2 *= strength;
			
			n2.vel += a1;
			n1.vel += a2;
			//nBase.vel -= a1 + a2;
			
			/*
			 d1 += a1 * 0.01;
			 d2 += a2 * 0.01;
			 float nd = Vector3::Dot(d1, d2) / (d1.GetLength() * d2.GetLength());
			 
			 if(dot > maxDot){
			 if(nd < dot)
			 printf("GOOD %f -> %f\n", dot, nd);
			 else
			 printf("BAD %f -> %f\n", dot, nd);
			 }else{
			 if(nd > dot)
			 printf("GOOD %f -> %f\n", dot, nd);
			 else
			 printf("BAD %f -> %f\n", dot, nd);
			 }*/
		}
		
		static float fractf(float v){
			return v - floorf(v);
		}
		
		static void CheckEscape(GameMap *map,
								IntVector3 hitBlock,
								IntVector3 a, IntVector3 b,
								IntVector3 dir,
								float& bestDist,
								IntVector3& bestDir) {
			hitBlock += dir;
			IntVector3 aa = a + dir;
			IntVector3 bb = b + dir;
			if(map->IsSolidWrapped(hitBlock.x, hitBlock.y, hitBlock.z))
				return;
			if(map->IsSolidWrapped(aa.x, aa.y, aa.z))
				return;
			if(map->IsSolidWrapped(bb.x, bb.y, bb.z))
				return;
			float dist;
			if(dir.x == 1) {
				dist = 1.f - fractf(a.x);
				dist += 1.f - fractf(b.x);
			}else if(dir.x == -1) {
				dist = fractf(a.x);
				dist += fractf(b.x);
			}else if(dir.y == 1) {
				dist = 1.f - fractf(a.y);
				dist += 1.f - fractf(b.y);
			}else if(dir.y == -1) {
				dist = fractf(a.y);
				dist += fractf(b.y);
			}else if(dir.z == 1) {
				dist = 1.f - fractf(a.z);
				dist += 1.f - fractf(b.z);
			}else if(dir.z == -1) {
				dist = fractf(a.z);
				dist += fractf(b.z);
			}else{
				SPAssert(false);
				return;
			}
			
			if(dist < bestDist){
				bestDist = dist;
				bestDir = dir;
			}
		}
		
		void Corpse::LineCollision(NodeType a, NodeType b, float dt){
			if(!r_corpseLineCollision)
				return;
			Node& n1 = nodes[a];
			Node& n2 = nodes[b];
			
			IntVector3 hitBlock;
			
			if(map->CastRay(n1.lastPos, n2.lastPos, 16.f, hitBlock)) {
				GameMap::RayCastResult res1 = map->CastRay2(n1.lastPos, n2.lastPos - n1.lastPos, 8);
				GameMap::RayCastResult res2 = map->CastRay2(n2.lastPos, n1.lastPos - n2.lastPos, 8);
				
				if(!res1.hit) return;
				if(!res2.hit) return;
				if(res1.startSolid || res2.startSolid){
					return;
				}
				
				// really hit?
				if(Vector3::Dot(res1.hitPos - n1.lastPos, n2.lastPos - n1.lastPos)
				   > (n2.pos-n1.pos).GetPoweredLength()){
					return;
				}
				if(Vector3::Dot(res1.hitPos - n1.lastPos, n2.lastPos - n1.lastPos)
				  < 0.f){
					return;
				}
				if(Vector3::Dot(res2.hitPos - n2.lastPos, n1.lastPos - n2.lastPos)
				   > (n2.pos-n1.pos).GetPoweredLength()){
					return;
				}
				if(Vector3::Dot(res2.hitPos - n2.lastPos, n1.lastPos - n2.lastPos)
				   < 0.f){
					return;
				}
				
				Vector3 blockPos;
				blockPos.x = hitBlock.x + .5f;
				blockPos.y = hitBlock.y + .5f;
				blockPos.z = hitBlock.z + .5f;
				
				float inlen = (res1.hitPos - res2.hitPos).GetLength();
				
				IntVector3 ivec = {0, 0, 0};
				
				ivec.x += res1.normal.x;
				ivec.y += res1.normal.y;
				ivec.z += res1.normal.z;
				ivec.x += res2.normal.x;
				ivec.y += res2.normal.y;
				ivec.z += res2.normal.z;
				
				Vector3 dir = {0.f, 0.f, 0.f};
				if(ivec.x == 0 && ivec.y == 0 && ivec.z == 0) {
					// hanging. which direction to escape?
					float bestDist = 1000.f;
					IntVector3 bestDir;
					CheckEscape(map, hitBlock,
								n1.pos.Floor(),
								n2.pos.Floor(),
								IntVector3::Make(1, 0, 0),
								bestDist, bestDir);
					CheckEscape(map, hitBlock,
								n1.pos.Floor(),
								n2.pos.Floor(),
								IntVector3::Make(-1, 0, 0),
								bestDist, bestDir);
					CheckEscape(map, hitBlock,
								n1.pos.Floor(),
								n2.pos.Floor(),
								IntVector3::Make(0, 1, 0),
								bestDist, bestDir);
					CheckEscape(map, hitBlock,
								n1.pos.Floor(),
								n2.pos.Floor(),
								IntVector3::Make(0, -1, 0),
								bestDist, bestDir);
					CheckEscape(map, hitBlock,
								n1.pos.Floor(),
								n2.pos.Floor(),
								IntVector3::Make(0, 0, 1),
								bestDist, bestDir);
					CheckEscape(map, hitBlock,
								n1.pos.Floor(),
								n2.pos.Floor(),
								IntVector3::Make(0, 0, -1),
								bestDist, bestDir);
					if(bestDist > 10.f){
						// failed to find appropriate direction.
						return;
					}
					ivec = bestDir;
					inlen = bestDist + .1f;
				}
				dir.x = ivec.x;
				dir.y = ivec.y;
				dir.z = ivec.z;
				
				Vector3 normDir = dir; // |D|
				
				n1.vel -= normDir * std::min(Vector3::Dot(normDir, n1.vel), 0.f);
				n2.vel -= normDir * std::min(Vector3::Dot(normDir, n2.vel), 0.f);
				
				dir *= dt * inlen * 5.f;
				
				n1.vel += dir;
				n2.vel += dir;
				
				// friction
				n1.vel -= (n1.vel - normDir * Vector3::Dot(normDir, n1.vel)) * .2f;
				n2.vel -= (n2.vel - normDir * Vector3::Dot(normDir, n2.vel)) * .2f;
				
			}
			
		}
		
		void Corpse::AngularMomentum(int eId,
									 NodeType a,
									 NodeType b){
			Edge& e = edges[eId];
			e.velDiff = nodes[b].vel - nodes[a].vel;
			if(e.node1 != a || e.node2 != b){
				e.lastVelDiff = e.velDiff;
				e.node1 = a; e.node2 = b;
				return;
			}
			
			Vector3 force = e.lastVelDiff - e.velDiff;
			force *= .5f;
			nodes[b].vel += force;
			nodes[a].vel -= force;
			
			e.lastVelDiff = e.velDiff;
		}

		void Corpse::ApplyConstraint(float dt) {
			SPADES_MARK_FUNCTION();
			
			
			AngularMomentum(0,
							Torso1, Torso2);
			AngularMomentum(1,
							Torso2, Torso3);
			AngularMomentum(2,
							Torso3, Torso4);
			AngularMomentum(3,
							Torso4, Torso1);
			AngularMomentum(4,
							Torso1, Arm1);
			AngularMomentum(5,
							Torso2, Arm2);
			AngularMomentum(6,
							Torso3, Leg1);
			AngularMomentum(7,
							Torso4, Leg2);
			
			Spring(Torso1, Torso2,
				   0.8f, dt);
			Spring(Torso3, Torso4,
				   0.8f, dt);
			
			Spring(Torso1, Torso4,
				   0.9f, dt);
			Spring(Torso2, Torso3,
				   0.9f, dt);
			
			Spring(Torso1, Torso3,
				   1.204f, dt);
			Spring(Torso2, Torso4,
				   1.204f, dt);
			
			
			Spring(Arm1, Torso1,
				   1.f, dt);
			Spring(Arm2, Torso2,
				   1.f, dt);
			Spring(Leg1, Torso3,
				   1.f, dt);
			Spring(Leg2, Torso4,
				   1.f, dt);
			
			AngleSpring(Torso1,
						Arm1, Torso3,
						-1.f, 0.6f, dt);
			AngleSpring(Torso2,
						Arm2, Torso4,
						-1.f, 0.6f, dt);
			
			AngleSpring(Torso3,
						Leg1, Torso2,
						-1.f, -0.2f, dt);
			AngleSpring(Torso4,
						Leg2, Torso1,
						-1.f, -0.2f, dt);
			
			Spring(Torso1, Torso2, Head,
				   .6f, dt);
			/*
			AngleSpring(Torso1,
						Torso2, Head,
						0.5f, 1.f, dt);
			
			AngleSpring(Torso2,
						Torso1, Head,
						0.5f, 1.f, dt);
			 */
			
			LineCollision(Torso1, Torso2, dt);
			LineCollision(Torso2, Torso3, dt);
			LineCollision(Torso3, Torso4, dt);
			LineCollision(Torso4, Torso1, dt);
			LineCollision(Torso1, Torso3, dt);
			LineCollision(Torso2, Torso4, dt);
			LineCollision(Torso1, Arm1, dt);
			LineCollision(Torso2, Arm2, dt);
			LineCollision(Torso3, Leg1, dt);
			LineCollision(Torso4, Leg2, dt);
			
			
			return;
			AngleSpring(Torso4,
						Torso1, Head,
						0.5f, 1.f, dt);
			
			AngleSpring(Torso3,
						Torso2, Head,
						0.5f, 1.f, dt);
			
			AngleSpring(Torso4,
						Torso2, Head,
						0.5f, 1.f, dt);
			
			AngleSpring(Torso3,
						Torso1, Head,
						0.5f, 1.f, dt);
		}
		
		void Corpse::Update(float dt) {
			SPADES_MARK_FUNCTION();
			float damp = 1.f;
            float damp2 = 1.f;
			if(dt > 0.f){
				damp = powf(.9f, dt);
				damp2 = powf(.371f, dt);
            }
			//dt *= 0.1f;
			
			for(int i = 0; i <NodeCount; i++){
				Node& node = nodes[i];
				Vector3 oldPos = node.lastPos;
				node.pos += node.vel * dt;
				
				SPAssert(!isnan(node.pos.x));
				SPAssert(!isnan(node.pos.y));
				SPAssert(!isnan(node.pos.z));
				
				if(node.pos.z > 63.f){
					node.vel.z -= dt * 6.f; // buoyancy
					node.vel *= damp;
				}else{
                    node.vel.z += dt * 32.f; // gravity
                    node.vel.z *= damp2;
                }
				
				//node.vel *= damp;
				
				if(!map->ClipBox(oldPos.x, oldPos.y, oldPos.z)){
					
					if(map->ClipBox(node.pos.x,
									oldPos.y,
									oldPos.z)){
						node.vel.x = -node.vel.x * .2f;
						if(fabsf(node.vel.x) < .3f)
							node.vel.x = 0.f;
						node.pos.x = oldPos.x;
						
						node.vel.y *= .5f;
						node.vel.z *= .5f;
					}
					
					if(map->ClipBox(node.pos.x,
									node.pos.y,
									oldPos.z)){
						node.vel.y = -node.vel.y * .2f;
						if(fabsf(node.vel.y) < .3f)
							node.vel.y = 0.f;
						node.pos.y = oldPos.y;
						
						node.vel.x *= .5f;
						node.vel.z *= .5f;
					}
					
					if(map->ClipBox(node.pos.x,
									node.pos.y,
									node.pos.z)){
						node.vel.z = -node.vel.z * .2f;
						if(fabsf(node.vel.z) < .3f)
							node.vel.z = 0.f;
						node.pos.z = oldPos.z;
						
						node.vel.x *= .5f;
						node.vel.y *= .5f;
					}
					
					if(map->ClipBox(node.pos.x, node.pos.y, node.pos.z)){
						// TODO: getting out block
						//node.pos = oldPos;
						//node.vel *= .5f;
					}
				}
				
				/*
				if(map->ClipBox(node.pos.x,
								node.pos.y,
								node.pos.z)){
					if(!map->ClipBox(node.pos.x,
									node.pos.y,
									oldPos.z)){
						node.vel.z = -node.vel.z * .2f;
						if(fabsf(node.vel.z) < .3f)
							node.vel.z = 0.f;
						node.pos.z = oldPos.z;
					}
					if(!map->ClipBox(node.pos.x,
									 oldPos.y,
									 node.pos.z)){
						node.vel.y = -node.vel.y * .2f;
						if(fabsf(node.vel.y) < .3f)
							node.vel.y = 0.f;
						node.pos.y = oldPos.y;
					}
					if(!map->ClipBox(oldPos.x,
									 node.pos.y,
									 node.pos.z)){
						node.vel.x = -node.vel.x * .2f;
						if(fabsf(node.vel.x) < .3f)
							node.vel.x = 0.f;
						node.pos.x = oldPos.x;
					}
					node.vel *= .8f;
					//node.pos = oldPos;
					
					if(node.vel.GetLength() < .02f){
						node.vel *= 0.f;
					}
				}*/
				
				node.lastPos = node.pos;
				node.lastForce = node.vel;
			}
			ApplyConstraint(dt);
			
			for(int i = 0; i <NodeCount; i++){
				nodes[i].lastForce = nodes[i].vel - nodes[i].lastForce;
			}
			
		}
		
		void Corpse::AddToScene() {
			if(false){
				// debug line only
				Vector4 col = {1, 1, 0, 0};
				renderer->AddDebugLine(nodes[Torso1].pos,
									   nodes[Torso2].pos,
									   col);
				renderer->AddDebugLine(nodes[Torso2].pos,
									   nodes[Torso3].pos,
									   col);
				renderer->AddDebugLine(nodes[Torso3].pos,
									   nodes[Torso4].pos,
									   col);
				renderer->AddDebugLine(nodes[Torso4].pos,
									   nodes[Torso1].pos,
									   col);
				
				renderer->AddDebugLine(nodes[Torso2].pos,
									   nodes[Torso4].pos,
									   col);
				renderer->AddDebugLine(nodes[Torso1].pos,
									   nodes[Torso3].pos,
									   col);
				
				
				renderer->AddDebugLine(nodes[Torso1].pos,
									   nodes[Arm1].pos,
									   col);
				renderer->AddDebugLine(nodes[Torso2].pos,
									   nodes[Arm2].pos,
									   col);
				
				renderer->AddDebugLine(nodes[Torso3].pos,
									   nodes[Leg1].pos,
									   col);
				renderer->AddDebugLine(nodes[Torso4].pos,
									   nodes[Leg2].pos,
									   col);
				
				
				renderer->AddDebugLine((nodes[Torso1].pos+nodes[Torso2].pos)*.5f,
									   nodes[Head].pos,
									   col);
				return;
			}
			
			ModelRenderParam param;
			param.customColor = color;
			
			IModel *model;
			Matrix4 scaler = Matrix4::Scale(.1f);
			
			// draw torso
			Matrix4 torso;
			Vector3 tX, tY;
			{
				Vector3 tX1 = nodes[Torso1].pos - nodes[Torso2].pos;
				Vector3 tX2 = nodes[Torso4].pos - nodes[Torso3].pos;
				Vector3 tY1 = nodes[Torso1].pos + nodes[Torso2].pos;
				Vector3 tY2 = nodes[Torso4].pos + nodes[Torso3].pos;
				tX = ((tX1 + tX2) * .5f).Normalize();
				tY = ((tY2 - tY1) * .5f).Normalize();
				Vector3 tZ = Vector3::Cross(tX, tY).Normalize();
				tY = Vector3::Cross(tX, tZ).Normalize();
				Vector3 tOrigin = tY1 * .5f;
				torso = Matrix4::FromAxis(tX, -tZ, -tY, tOrigin);
				
				param.matrix = torso * scaler;
				
				model = renderer->RegisterModel
				("Models/Player/Torso.kv6");
				renderer->RenderModel(model, param);
			}
			// draw Head
			{
				Vector3 headBase =
				(torso * MakeVector3(0.0f, 0.f, 0.f)).GetXYZ();
				
				model = renderer->RegisterModel
				("Models/Player/Head.kv6");
				
				Vector3 aX, aY, aZ;
				Vector3 center = (nodes[Torso1].pos + nodes[Torso2].pos) * .5f;
				
				aZ = nodes[Head].pos - center;
				aZ = -torso.GetAxis(2);
				aZ = aZ.Normalize();
				aY = nodes[Torso2].pos - nodes[Torso1].pos;
				aY = Vector3::Cross(aY, aZ).Normalize();
				aX = Vector3::Cross(aY, aZ).Normalize();
				param.matrix = Matrix4::FromAxis(-aX, aY, -aZ, headBase) * scaler;
				
				renderer->RenderModel(model, param);
			}
			
			// draw Arms
			{
				Vector3 arm1Base =
				(torso * MakeVector3(0.4f, 0.f, 0.2f)).GetXYZ();
				Vector3 arm2Base =
				(torso * MakeVector3(-0.4f, 0.f, 0.2f)).GetXYZ();
				
				model = renderer->RegisterModel
				("Models/Player/Arm.kv6");
				
				Vector3 aX, aY, aZ;
				
				aZ = nodes[Arm1].pos - nodes[Torso1].pos;
				aZ = aZ.Normalize();
				aY = nodes[Torso2].pos - nodes[Torso1].pos;
				aY = Vector3::Cross(aY, aZ).Normalize();
				aX = Vector3::Cross(aY, aZ).Normalize();
				param.matrix = Matrix4::FromAxis(aX, aY, aZ, arm1Base) * scaler;
				
				renderer->RenderModel(model, param);
				
				aZ = nodes[Arm2].pos - nodes[Torso2].pos;
				aZ = aZ.Normalize();
				aY = nodes[Torso1].pos - nodes[Torso2].pos;
				aY = Vector3::Cross(aY, aZ).Normalize();
				aX = Vector3::Cross(aY, aZ).Normalize();
				param.matrix = Matrix4::FromAxis(aX, aY, aZ, arm2Base) * scaler;
				
				renderer->RenderModel(model, param);
			}
			
			// draw Leg
			{
				Vector3 leg1Base =
				(torso * MakeVector3(0.25f, 0.f, 0.9f)).GetXYZ();
				Vector3 leg2Base =
				(torso * MakeVector3(-0.25f, 0.f, 0.9f)).GetXYZ();
				
				model = renderer->RegisterModel
				("Models/Player/Leg.kv6");
				
				Vector3 aX, aY, aZ;
				
				aZ = nodes[Leg1].pos - nodes[Torso3].pos;
				aZ = aZ.Normalize();
				aY = nodes[Torso1].pos - nodes[Torso2].pos;
				aY = Vector3::Cross(aY, aZ).Normalize();
				aX = Vector3::Cross(aY, aZ).Normalize();
				param.matrix = Matrix4::FromAxis(aX, aY, aZ, leg1Base) * scaler;
				
				renderer->RenderModel(model, param);
				
				aZ = nodes[Leg2].pos - nodes[Torso4].pos;
				aZ = aZ.Normalize();
				aY = nodes[Torso1].pos - nodes[Torso2].pos;
				aY = Vector3::Cross(aY, aZ).Normalize();
				aX = Vector3::Cross(aY, aZ).Normalize();
				param.matrix = Matrix4::FromAxis(aX, aY, aZ, leg2Base) * scaler;
				
				renderer->RenderModel(model, param);
			}
		}
		
		Vector3 Corpse::GetCenter() {
			Vector3 v = {0,0,0};
			for(int i = 0; i < NodeCount; i++)
				v += nodes[i].pos;
			v *= 1.f / (float)NodeCount;
			return v;
		}
		
		bool Corpse::IsVisibleFrom(spades::Vector3 eye){
			// distance culled?
			if((eye - GetCenter()).GetLength() > 150.f)
				return false;
			
			for(int i = 0; i < NodeCount; i++){
				IntVector3 outBlk;
				if(map->CastRay(eye, nodes[i].pos,
								 256.f, outBlk))
					return true;
			}
			return false;
		}
		
		void Corpse::AddImpulse(spades::Vector3 v){
			for(int i = 0; i < NodeCount; i++)
				nodes[i].vel += v;
		}
	}
}