void ModelBody::RebuildCollisionMesh()
{
	if (m_geom) {
		if (GetFrame()) RemoveGeomsFromFrame(GetFrame());
		DeleteGeoms();
	}

	m_collMesh = m_model->GetCollisionMesh();
	SetPhysRadius(m_collMesh->GetAabb().GetRadius());

	//static geom
	m_geom = new Geom(m_collMesh->GetGeomTree());
	m_geom->SetUserData(static_cast<void*>(this));
	m_geom->MoveTo(GetOrient(), GetPosition());

	//have to figure out which collision geometries are responsible for which geomtrees
	DynGeomFinder dgf;
	m_model->GetRoot()->Accept(dgf);

	//dynamic geoms
	for (auto it = m_collMesh->GetDynGeomTrees().begin(); it != m_collMesh->GetDynGeomTrees().end(); ++it) {
		Geom *dynG = new Geom(*it);
		dynG->SetUserData(static_cast<void*>(this));
		dynG->MoveTo(GetOrient(), GetPosition());
		dynG->m_animTransform = matrix4x4d::Identity();
		SceneGraph::CollisionGeometry *cg = dgf.GetCgForTree(*it);
		if (cg)
			cg->SetGeom(dynG);
		m_dynGeoms.push_back(dynG);
	}

	if (GetFrame()) AddGeomsToFrame(GetFrame());
}

void Ship::TestLanded()
{
	m_testLanded = false;
	if (m_launchLockTimeout > 0.0f) return;
	if (m_wheelState < 1.0f) return;
	if (GetFrame()->GetBody()->IsType(Object::PLANET)) {
		double speed = GetVelocity().Length();
		vector3d up = GetPosition().Normalized();
		const double planetRadius = static_cast<Planet*>(GetFrame()->GetBody())->GetTerrainHeight(up);

		if (speed < MAX_LANDING_SPEED) {
			// check player is sortof sensibly oriented for landing
			if (GetOrient().VectorY().Dot(up) > 0.99) {
				// position at zero altitude
				SetPosition(up * (planetRadius - GetAabb().min.y));

				// position facing in roughly the same direction
				vector3d right = up.Cross(GetOrient().VectorZ()).Normalized();
				SetOrient(matrix3x3d::FromVectors(right, up));

				SetVelocity(vector3d(0, 0, 0));
				SetAngVelocity(vector3d(0, 0, 0));
				ClearThrusterState();
				SetFlightState(LANDED);
				Sound::BodyMakeNoise(this, "Rough_Landing", 1.0f);
				LuaEvent::Queue("onShipLanded", this, GetFrame()->GetBody());
				onLanded.emit();
			}
		}
	}
}

// Input: direction in ship's frame, doesn't need to be normalized
// Approximate positive angular velocity at match point
// Applies thrust directly
// old: returns whether it can reach that direction in this frame
// returns angle to target
double Ship::AIFaceDirection(const vector3d &dir, double av)
{
	double maxAccel = m_type->angThrust / GetAngularInertia();		// should probably be in stats anyway

	vector3d head = (dir * GetOrient()).Normalized();		// create desired object-space heading
	vector3d dav(0.0, 0.0, 0.0);	// desired angular velocity

	double ang = 0.0;
	if (head.z > -0.99999999)
	{
		ang = acos (Clamp(-head.z, -1.0, 1.0));		// scalar angle from head to curhead
		double iangvel = av + calc_ivel_pos(ang, 0.0, maxAccel);	// ideal angvel at current time

		// Normalize (head.x, head.y) to give desired angvel direction
		if (head.z > 0.999999) head.x = 1.0;
		double head2dnorm = 1.0 / sqrt(head.x*head.x + head.y*head.y);		// NAN fix shouldn't be necessary if inputs are normalized
		dav.x = head.y * head2dnorm * iangvel;
		dav.y = -head.x * head2dnorm * iangvel;
	}
	vector3d cav = GetAngVelocity() * GetOrient();				// current obj-rel angvel
	double frameAccel = maxAccel * Pi::game->GetTimeStep();
	vector3d diff = (dav - cav) / frameAccel;	// find diff between current & desired angvel

	// If the player is pressing a roll key, don't override roll.
	// XXX this really shouldn't be here. a better way would be to have a
	// field in Ship describing the wanted angvel adjustment from input. the
	// baseclass version in Ship would always be 0. the version in Player
	// would be constructed from user input. that adjustment could then be
	// considered by this method when computing the required change
	if (IsType(Object::PLAYER) && (KeyBindings::rollLeft.IsActive() || KeyBindings::rollRight.IsActive()))
		diff.z = m_angThrusters.z;
	SetAngThrusterState(diff);
	return ang;
}

bool Ship::FireMissile(int idx, Ship *target)
{
	assert(target);

	if (GetFlightState() != FLYING) return false;

	const Equip::Type t = m_equipment.Get(Equip::SLOT_MISSILE, idx);
	if (t == Equip::NONE) {
		return false;
	}

	m_equipment.Set(Equip::SLOT_MISSILE, idx, Equip::NONE);
	UpdateEquipStats();

	ShipType::Id mtype;
	switch (t) {
		case Equip::MISSILE_SMART: mtype = ShipType::MISSILE_SMART; break;
		case Equip::MISSILE_NAVAL: mtype = ShipType::MISSILE_NAVAL; break;
		case Equip::MISSILE_UNGUIDED: mtype = ShipType::MISSILE_UNGUIDED; break;
		default:
		case Equip::MISSILE_GUIDED: mtype = ShipType::MISSILE_GUIDED; break;
	}
	Missile *missile = new Missile(mtype, this, target);
	missile->SetOrient(GetOrient());
	missile->SetFrame(GetFrame());
	// XXX DODGY! need to put it in a sensible location
	vector3d dir = -GetOrient().VectorZ();
	missile->SetPosition(GetPosition()+50.0*dir);
	missile->SetVelocity(GetVelocity());
	Pi::game->GetSpace()->AddBody(missile);
	return true;
}

void SpaceStation::UpdateInterpTransform(double alpha)
{
	double len = m_oldAngDisplacement * (1.0-alpha);
	if (!is_zero_exact(len)) {
		matrix3x3d rot = matrix3x3d::RotateY(len);		// RotateY is backwards
		m_interpOrient = rot * GetOrient();
	}
	else m_interpOrient = GetOrient();
	m_interpPos = GetPosition();
}

bool Ship::SpawnCargo(CargoBody * c_body) const
{
	if (m_flightState != FLYING) return false;
	vector3d pos = GetOrient() * vector3d(0, GetAabb().min.y - 5, 0);
	c_body->SetFrame(GetFrame());
	c_body->SetPosition(GetPosition() + pos);
	c_body->SetVelocity(GetVelocity() + GetOrient()*vector3d(0, -10, 0));
	Pi::game->GetSpace()->AddBody(c_body);
	return true;
}

void DynamicBody::UpdateInterpTransform(double alpha)
{
    m_interpPos = alpha*GetPosition() + (1.0-alpha)*m_oldPos;

    double len = m_oldAngDisplacement.Length() * (1.0-alpha);
    if (len > 1e-16) {
        vector3d axis = m_oldAngDisplacement.Normalized();
        matrix3x3d rot = matrix3x3d::Rotate(-len, axis);		// rotate backwards
        m_interpOrient = rot * GetOrient();
    }
    else m_interpOrient = GetOrient();
}

bool SpaceStation::OnCollision(Object *b, Uint32 flags, double relVel)
{
	if ((flags & 0x10) && (b->IsType(Object::SHIP))) {
		Ship *s = static_cast<Ship*>(b);

		int port = -1;
		for (int i=0; i<MAX_DOCKING_PORTS; i++) {
			if (m_shipDocking[i].ship == s) { port = i; break; }
		}
		if (port == -1) return false;					// no permission
		if (!m_type->dockOneAtATimePlease) {
			if (port != int(flags & 0xf)) return false;		// wrong port
		}
		if (m_shipDocking[port].stage != 1) return false;	// already docking?

		SpaceStationType::positionOrient_t dport;
		// why stage 2? Because stage 1 is permission to dock
		// granted, stage 2 is start of docking animation.
		PiVerify(m_type->GetDockAnimPositionOrient(port, 2, 0.0, vector3d(0.0), dport, s));

		// must be oriented sensibly and have wheels down
		if (IsGroundStation()) {
			vector3d dockingNormal = GetOrient()*dport.yaxis;
			const double dot = s->GetOrient().VectorY().Dot(dockingNormal);
			if ((dot < 0.99) || (s->GetWheelState() < 1.0)) return false;	// <0.99 harsh?
			if (s->GetVelocity().Length() > MAX_LANDING_SPEED) return false;
		}

		// if there is more docking port anim to do, don't set docked yet
		if (m_type->numDockingStages >= 2) {
			shipDocking_t &sd = m_shipDocking[port];
			sd.ship = s;
			sd.stage = 2;
			sd.stagePos = 0;
			sd.fromPos = (s->GetPosition() - GetPosition()) * GetOrient();	// station space
			sd.fromRot = Quaterniond::FromMatrix3x3(GetOrient().Transpose() * s->GetOrient());
			if (m_type->dockOneAtATimePlease) m_dockingLock = true;

			s->SetFlightState(Ship::DOCKING);
			s->SetVelocity(vector3d(0.0));
			s->SetAngVelocity(vector3d(0.0));
			s->ClearThrusterState();
		} else {
			s->SetDockedWith(this, port);				// bounces back to SS::SetDocked()
			LuaEvent::Queue("onShipDocked", s, this);
		}
		return false;
	} else {
		return true;
	}
}

Missile * Ship::SpawnMissile(ShipType::Id missile_type, int power) {
	if (GetFlightState() != FLYING)
		return 0;

	Missile *missile = new Missile(missile_type, this, power);
	missile->SetOrient(GetOrient());
	missile->SetFrame(GetFrame());
	// XXX DODGY! need to put it in a sensible location
	vector3d dir = -GetOrient().VectorZ();
	missile->SetPosition(GetPosition()+50.0*dir);
	missile->SetVelocity(GetVelocity());
	Pi::game->GetSpace()->AddBody(missile);
	return missile;
}

Missile * Ship::SpawnMissile(ShipType::Id missile_type, int power) {
	if (GetFlightState() != FLYING)
		return 0;

	Missile *missile = new Missile(missile_type, this, power);
	missile->SetOrient(GetOrient());
	missile->SetFrame(GetFrame());
	const vector3d pos = GetOrient() * vector3d(0, GetAabb().min.y - 10, GetAabb().min.z);
	const vector3d vel = -40.0 * GetOrient().VectorZ();
	missile->SetPosition(GetPosition()+pos);
	missile->SetVelocity(GetVelocity()+vel);
	Pi::game->GetSpace()->AddBody(missile);
	return missile;
}

bool SpaceStation::LaunchShip(Ship *ship, int port)
{
	shipDocking_t &sd = m_shipDocking[port];
	if (sd.stage < 0) return true;			// already launching
	if (m_dockingLock) return false;		// another ship docking
	if (m_type->dockOneAtATimePlease) m_dockingLock = true;

	sd.ship = ship;
	sd.stage = -1;
	sd.stagePos = 0;
	sd.fromPos = (ship->GetPosition() - GetPosition()) * GetOrient();	// station space
	sd.fromRot = Quaterniond::FromMatrix3x3(GetOrient().Transpose() * ship->GetOrient());

	ship->SetFlightState(Ship::DOCKING);
	return true;
}

void SpaceStation::TimeStepUpdate(const float timeStep)
{
	// rotate the thing
	double len = m_type->angVel * timeStep;
	if (!is_zero_exact(len)) {
		matrix3x3d r = matrix3x3d::RotateY(-len);		// RotateY is backwards
		SetOrient(r * GetOrient());
	}
	m_oldAngDisplacement = len;

	// reposition the ships that are docked or docking here
	for (int i=0; i<m_type->numDockingPorts; i++) {
		const shipDocking_t &dt = m_shipDocking[i];
		if (!dt.ship) { //free
			m_navLights->SetColor(i+1, NavLights::NAVLIGHT_GREEN);
			continue;
		}
		if (dt.stage == 1) //reserved
			m_navLights->SetColor(i+1, NavLights::NAVLIGHT_YELLOW);
		if (dt.ship->GetFlightState() == Ship::FLYING)
			continue;
		PositionDockedShip(dt.ship, i);
		m_navLights->SetColor(i+1, NavLights::NAVLIGHT_RED); //docked
	}

	if (m_doorAnimation)
		GetModel()->UpdateAnimations();
}

void DynamicBody::TimeStepUpdate(const float timeStep)
{
    m_oldPos = GetPosition();
    if (m_isMoving) {
        m_force += m_externalForce;

        m_vel += double(timeStep) * m_force * (1.0 / m_mass);
        m_angVel += double(timeStep) * m_torque * (1.0 / m_angInertia);

        double len = m_angVel.Length();
        if (len > 1e-16) {
            vector3d axis = m_angVel * (1.0 / len);
            matrix3x3d r = matrix3x3d::Rotate(len * timeStep, axis);
            SetOrient(r * GetOrient());
        }
        m_oldAngDisplacement = m_angVel * timeStep;

        SetPosition(GetPosition() + m_vel * double(timeStep));

//if (this->IsType(Object::PLAYER))
//Output("pos = %.1f,%.1f,%.1f, vel = %.1f,%.1f,%.1f, force = %.1f,%.1f,%.1f, external = %.1f,%.1f,%.1f\n",
//	pos.x, pos.y, pos.z, m_vel.x, m_vel.y, m_vel.z, m_force.x, m_force.y, m_force.z,
//	m_externalForce.x, m_externalForce.y, m_externalForce.z);

        m_lastForce = m_force;
        m_lastTorque = m_torque;
        m_force = vector3d(0.0);
        m_torque = vector3d(0.0);
        CalcExternalForce();			// regenerate for new pos/vel
    } else {
        m_oldAngDisplacement = vector3d(0.0);
    }

    ModelBody::TimeStepUpdate(timeStep);
}

// Change object-space velocity in direction of param
vector3d Ship::AIChangeVelDir(const vector3d &reqdiffvel)
{
	// get max thrust in desired direction after external force compensation
	vector3d maxthrust = GetMaxThrust(reqdiffvel);
	maxthrust += GetExternalForce() * GetOrient();
	vector3d maxFA = maxthrust * (Pi::game->GetTimeStep() / GetMass());
	maxFA.x = fabs(maxFA.x); maxFA.y = fabs(maxFA.y); maxFA.z = fabs(maxFA.z);

	// crunch diffvel by relative thruster power to get acceleration in right direction
	vector3d diffvel = reqdiffvel;
	if (fabs(diffvel.x) > maxFA.x) diffvel *= maxFA.x / fabs(diffvel.x);
	if (fabs(diffvel.y) > maxFA.y) diffvel *= maxFA.y / fabs(diffvel.y);
	if (fabs(diffvel.z) > maxFA.z) diffvel *= maxFA.z / fabs(diffvel.z);

	AIChangeVelBy(diffvel);		// should always return true because it's already capped?
	return GetOrient() * (reqdiffvel - diffvel);		// should be remaining diffvel to correct
}

void ModelBody::SetPosition(const vector3d &p)
{
	Body::SetPosition(p);
	MoveGeoms(GetOrient(), p);

	// for rebuild of static objects in collision space
	if (m_isStatic) SetFrame(GetFrame());
}

void ModelBody::SetPosition(const vector3d &p)
{
	Body::SetPosition(p);
	if (!m_geom) return;
	matrix4x4d m2 = GetOrient();
	m_geom->MoveTo(m2, p);
	// for rebuild of static objects in collision space
	if (m_isStatic) SetFrame(GetFrame());
}

void Ship::AIAccelToModelRelativeVelocity(const vector3d v)
{
	vector3d difVel = v - GetVelocity() * GetOrient();		// required change in velocity
	vector3d maxThrust = GetMaxThrust(difVel);
	vector3d maxFrameAccel = maxThrust * (Pi::game->GetTimeStep() / GetMass());

	SetThrusterState(0, difVel.x / maxFrameAccel.x);
	SetThrusterState(1, difVel.y / maxFrameAccel.y);
	SetThrusterState(2, difVel.z / maxFrameAccel.z);	// use clamping
}

void Body::SwitchToFrame(Frame *newFrame)
{
	vector3d vel = GetVelocityRelTo(newFrame);		// do this first because it uses position
	vector3d fpos = m_frame->GetPositionRelTo(newFrame);
	matrix3x3d forient = m_frame->GetOrientRelTo(newFrame);
	SetPosition(forient * GetPosition() + fpos);
	SetOrient(forient * GetOrient());
	SetVelocity(vel + newFrame->GetStasisVelocity(GetPosition()));
	SetFrame(newFrame);

	LuaEvent::Queue("onFrameChanged", this);
}

bool Ship::OnDamage(Object *attacker, float kgDamage, const CollisionContact& contactData)
{
	if (m_invulnerable) {
		Sound::BodyMakeNoise(this, "Hull_hit_Small", 0.5f);
		return true;
	}

	if (!IsDead()) {
		float dam = kgDamage*0.001f;
		if (m_stats.shield_mass_left > 0.0f) {
			if (m_stats.shield_mass_left > dam) {
				m_stats.shield_mass_left -= dam;
				dam = 0;
			} else {
				dam -= m_stats.shield_mass_left;
				m_stats.shield_mass_left = 0;
			}
			Properties().Set("shieldMassLeft", m_stats.shield_mass_left);
		}

		m_shieldCooldown = DEFAULT_SHIELD_COOLDOWN_TIME;
		// transform the collision location into the models local space (from world space) and add it as a hit.
		matrix4x4d mtx = GetOrient();
		mtx.SetTranslate( GetPosition() );
		const matrix4x4d invmtx = mtx.Inverse();
		const vector3d localPos = invmtx * contactData.pos;
		GetShields()->AddHit(localPos);

		m_stats.hull_mass_left -= dam;
		Properties().Set("hullMassLeft", m_stats.hull_mass_left);
		Properties().Set("hullPercent", 100.0f * (m_stats.hull_mass_left / float(m_type->hullMass)));
		if (m_stats.hull_mass_left < 0) {
			if (attacker) {
				if (attacker->IsType(Object::BODY))
					LuaEvent::Queue("onShipDestroyed", this, dynamic_cast<Body*>(attacker));
			}

			Explode();
		} else {
			if (Pi::rng.Double() < kgDamage)
				SfxManager::Add(this, TYPE_DAMAGE);

			if (dam < 0.01 * float(GetShipType()->hullMass))
				Sound::BodyMakeNoise(this, "Hull_hit_Small", 1.0f);
			else
				Sound::BodyMakeNoise(this, "Hull_Hit_Medium", 1.0f);
		}
	}

	//Output("Ouch! %s took %.1f kilos of damage from %s! (%.1f t hull left)\n", GetLabel().c_str(), kgDamage, attacker->GetLabel().c_str(), m_stats.hull_mass_left);
	return true;
}

void Missile::StaticUpdate(const float timeStep)
{

	// Note: direct call to AI->TimeStepUpdate
	if (m_curAICmd!=nullptr) m_curAICmd->TimeStepUpdate();
	//Add smoke trails for missiles on thruster state
	static double s_timeAccum = 0.0;
	s_timeAccum += timeStep;
	if (!is_equal_exact(GetThrusterState().LengthSqr(), 0.0) && (s_timeAccum > 4 || 0.1*Pi::rng.Double() < timeStep)) {
		s_timeAccum = 0.0;
		const vector3d pos = GetOrient() * vector3d(0, 0 , 5);
		const float speed = std::min(10.0*GetVelocity().Length()*std::max(1.0,fabs(GetThrusterState().z)),100.0);
		SfxManager::AddThrustSmoke(this, speed, pos);
	}
}