int G_begin_rhumbline_equation(double lon1, double lat1, double lon2,
			       double lat2)
{
    adjust_lat(&lat1);
    adjust_lat(&lat2);

    if (lon1 == lon2) {
	st->parallel = 1;		/* a lie */
	st->L = lat1;
	return 0;
    }
    if (lat1 == lat2) {
	st->parallel = 1;
	st->L = lat1;
	return 1;
    }
    st->parallel = 0;
    lon1 = Radians(lon1);
    lon2 = Radians(lon2);
    lat1 = Radians(lat1);
    lat2 = Radians(lat2);

    st->TAN1 = tan(M_PI_4 + lat1 / 2.0);
    st->TAN2 = tan(M_PI_4 + lat2 / 2.0);
    st->TAN_A = (lon2 - lon1) / (log(st->TAN2) - log(st->TAN1));
    st->L = lon1;

    return 1;
}

void handleLightControls()
{
    const Vector lightUp(0.0, 1.0, 0.0);

    if (sf::Keyboard::isKeyPressed(sf::Keyboard::Up))
    {
        lightPos = lightPos.Rotate(lightAxis, Radians(lightRotationSpeed));
        lightPos2 = lightPos2.Rotate(lightAxis2, Radians(lightRotationSpeed));
    }

    if (sf::Keyboard::isKeyPressed(sf::Keyboard::Down))
    {
        lightPos = lightPos.Rotate(lightAxis, Radians(-lightRotationSpeed));
        lightPos2 = lightPos2.Rotate(lightAxis2, Radians(-lightRotationSpeed));
    }

    if (sf::Keyboard::isKeyPressed(sf::Keyboard::Left))
    {
        lightPos = lightPos.Rotate(lightUp, Radians(lightRotationSpeed));
        lightPos2 = lightPos2.Rotate(lightUp, Radians(lightRotationSpeed));
        lightAxis = lightAxis.Rotate(lightUp, Radians(lightRotationSpeed));
    }

    if (sf::Keyboard::isKeyPressed(sf::Keyboard::Right))
    {
        lightPos = lightPos.Rotate(lightUp, Radians(-lightRotationSpeed));
        lightPos2 = lightPos2.Rotate(lightUp, Radians(-lightRotationSpeed));
        lightAxis = lightAxis.Rotate(lightUp, Radians(-lightRotationSpeed));
    }
}

    void Node::ShowGUIProperties(Editor* editor)
    {
        std::string header = "Transform:" + GetName();
        if (ImGui::TreeNode(header.c_str()))
        {
            auto position = GetPosition();
            ImGui::DragFloat3("Position", &position[0], 0.1f);
            SetPosition(position);

            auto guiRotation = GetGUIRotation();
            auto oldRotation = Radians(guiRotation);
            ImGui::DragFloat3("Rotation", &guiRotation[0], 1, 0, 360);
            auto rad = Radians(guiRotation);
            auto q = GetOrientation();
            q *= Inverse(Quaternion(oldRotation)) * Quaternion(rad);
            SetOrientation(q);
            SetGUIRotation(guiRotation);

            auto scale = GetScale();
            ImGui::DragFloat3("Scale", &scale[0], 0.1f);
            SetScale(scale);

            ImGui::TreePop();
        }
    }

Transform Rotate(float angle, const Vector &axis) {
    Vector a = Normalize(axis);
    float s = sinf(Radians(angle));
    float c = cosf(Radians(angle));
    float m[4][4];

    m[0][0] = a.x * a.x + (1.f - a.x * a.x) * c;
    m[0][1] = a.x * a.y * (1.f - c) - a.z * s;
    m[0][2] = a.x * a.z * (1.f - c) + a.y * s;
    m[0][3] = 0;

    m[1][0] = a.x * a.y * (1.f - c) + a.z * s;
    m[1][1] = a.y * a.y + (1.f - a.y * a.y) * c;
    m[1][2] = a.y * a.z * (1.f - c) - a.x * s;
    m[1][3] = 0;

    m[2][0] = a.x * a.z * (1.f - c) - a.y * s;
    m[2][1] = a.y * a.z * (1.f - c) + a.x * s;
    m[2][2] = a.z * a.z + (1.f - a.z * a.z) * c;
    m[2][3] = 0;

    m[3][0] = 0;
    m[3][1] = 0;
    m[3][2] = 0;
    m[3][3] = 1;

    Matrix4x4 mat(m);
    return Transform(mat, Transpose(mat));
}

int main(void)
{
	cout.precision(3);
	cout<<"Wheel spinning at 1 radian per iteration:"<<endl<<"Position"<<endl;
	Delta<Radians> rotationalVelocity;
	for(rotationalVelocity = Radians(1.0); rotationalVelocity.get() < 10.0; rotationalVelocity.increment())
		cout<<double(rotationalVelocity.get())<<endl;
	
	cout<<"Now lets start trying to spin it the other way. Every 5 iterations we'll decrease the speed by one third of a radian per iteration"<<endl<<"Position\tVelocity"<<endl;
	for(int i = 0; i < 4; i++)
	{
		rotationalVelocity -= 2.0/3.0;
		for(int j = 0; j < 5; j++)
		{
			cout<<double(rotationalVelocity.get())<<'\t'<<'\t'<<double(rotationalVelocity)<<endl;
			rotationalVelocity.increment();
		}
	}
	
	cout<<"Das p cool, but I want continuity. Let's increase the Delta every instant, using a delta"<<endl<<"Position\tVelocity\tAcceleration"<<endl;
	for(Delta<Delta<Radians> > rotationalAccel(Delta<Radians>(Radians(0.2)), rotationalVelocity); rotationalAccel.get() < 100.0; rotationalAccel.increment())
		cout<<double(rotationalAccel.get(Delta<Radians>::N_DERIVATIVE))<<'\t'<<'\t'<<
		double(rotationalAccel.get(Delta<Delta<Radians> >::N_DERIVATIVE))<<'\t'<<'\t'<<
		double(rotationalAccel)<<endl;
	
	cout<<"Wowee. I wonder what else this thing can do..."<<endl;
	return 0;
}

int G_begin_geodesic_equation(double lon1, double lat1, double lon2,
			      double lat2)
{
    double sin21, tan1, tan2;

    adjust_lon(&lon1);
    adjust_lon(&lon2);
    adjust_lat(&lat1);
    adjust_lat(&lat2);
    if (lon1 > lon2) {
	double temp;
	temp = lon1; lon1 = lon2; lon2 = temp;
	temp = lat1; lat1 = lat2; lat2 = temp;
    }
    if (lon1 == lon2) {
	st->A = st->B = 0.0;
	return 0;
    }
    lon1 = Radians(lon1);
    lon2 = Radians(lon2);
    lat1 = Radians(lat1);
    lat2 = Radians(lat2);

    sin21 = sin(lon2 - lon1);
    tan1 = tan(lat1);
    tan2 = tan(lat2);

    st->A = (tan2 * cos(lon1) - tan1 * cos(lon2)) / sin21;
    st->B = (tan2 * sin(lon1) - tan1 * sin(lon2)) / sin21;

    return 1;
}

//! renvoie la transformation associee a une camera perspective (Projection).
Transform Perspective( float fov, float aspect, float znear, float zfar )
{
#if 0
    // Perform projective divide, pbrt version
    float inv_denom = 1.f / ( zfar - znear );
    Matrix4x4 persp( 
        1, 0,           0,              0,
        0, 1,           0,              0,
        0, 0, zfar*inv_denom, -zfar*znear*inv_denom,
        0, 0,           1,              0 
    );
    
    // Scale to canonical viewing volume
    float invTanAng = 1.f / tanf( Radians( fov ) / 2.f );
    return Scale( invTanAng, invTanAng, 1 ) * Transform( persp );

#else
    // perspective, openGL version
    const float inv_tan= 1.f / tanf(Radians(fov) * 0.5f);
    const float inv_denom= 1.f / (znear - zfar);
    Matrix4x4 persp( 
        inv_tan/aspect,       0,                    0,                      0,
                     0, inv_tan,                    0,                      0,
                     0,       0, (zfar+znear)*inv_denom, 2.f*zfar*znear*inv_denom,
                     0,       0,                   -1,                      0
    );

    return Transform(persp);
#endif
}

void Projectile_Clusterbomb::Explode()
{

	Projectile::Explode();

	// Throw bomblets
	const int BOMBLETS					= 5;
	const float START_ANGLE				= Radians(30.0f);
	const float ANGLE_STEP				= (Math::PI - 2.0f * START_ANGLE) / (float)BOMBLETS;
	const float ANGLE_RANDOMNESS		= Radians(10.0f);
	const float SPEED_MIN				= 200.0f;
	const float SPEED_MAX				= 500.0f;
	const ExplosionData EXPLOSION_DATA	= ExplosionData(30.0f, 35.0f, 200.0f, 30.0f, 30.0f);

	float angle = START_ANGLE;
	for (int i = 0; i < BOMBLETS; ++ i)
	{

		float adjustedAngle = angle + Random::RandomFloat(-ANGLE_RANDOMNESS, ANGLE_RANDOMNESS);

		Projectile_Clusterbomblet* projectile = new Projectile_Clusterbomblet();
		projectile->SetImage(ResourceManager::Get()->GetImage("image_grenade"));
		projectile->SetBounds(5.0f, 5.0f);
		projectile->SetPosition(GetPosition());
		projectile->SetExplosionData(EXPLOSION_DATA);
		projectile->SetVelocity(Vec2f(Cos(adjustedAngle), Sin(adjustedAngle)) * Random::RandomFloat(SPEED_MIN, SPEED_MAX));
		projectile->SetHitpoints(1000);

		Game::Get()->GetWorld()->AddCreatedObject(projectile);

		angle += ANGLE_STEP;

	}

}

int G_begin_geodesic_equation(double lon1, double lat1, double lon2,
			      double lat2)
{
    double sin21, tan1, tan2;

    adjust_lon(&lon1);
    adjust_lon(&lon2);
    adjust_lat(&lat1);
    adjust_lat(&lat2);
    if (lon1 > lon2) {
	register double temp;

	SWAP(lon1, lon2);
	SWAP(lat1, lat2);
    }
    if (lon1 == lon2) {
	A = B = 0.0;
	return 0;
    }
    lon1 = Radians(lon1);
    lon2 = Radians(lon2);
    lat1 = Radians(lat1);
    lat2 = Radians(lat2);

    sin21 = sin(lon2 - lon1);
    tan1 = tan(lat1);
    tan2 = tan(lat2);

    A = (tan2 * cos(lon1) - tan1 * cos(lon2)) / sin21;
    B = (tan2 * sin(lon1) - tan1 * sin(lon2)) / sin21;

    return 1;
}

// SpotLight Method Definitions
SpotLight::SpotLight(const Transform &LightToWorld, const Medium *medium,
                     const Spectrum &intensity, Float width, Float fall)
    : Light(LightFlags::DeltaPosition, LightToWorld, medium),
      pLight(LightToWorld(Point3f(0, 0, 0))),
      intensity(intensity),
      cosTotalWidth(std::cos(Radians(width))),
      cosFalloffStart(std::cos(Radians(fall))) {}

// SpotLight Method Definitions
SpotLight::SpotLight(const Transform &light2world,
                     const Spectrum &intensity, float width, float fall)
    : Light(light2world) {
    lightPos = LightToWorld(Point(0,0,0));
    Intensity = intensity;
    cosTotalWidth = cosf(Radians(width));
    cosFalloffStart = cosf(Radians(fall));
}

// 足の座標を設定
void Foot::Move(Vec2 new_upper_foot, float new_angle)
{
    upper_foot.pos = new_upper_foot;
    lower_foot.pos = { 
        upper_foot.pos.x + cos(Radians(upper_foot.angle + 90)) * (upper_foot.size.y / 2),
        upper_foot.pos.y + (upper_foot.size.y / 2) - (upper_foot.size.x / 2) + sin(Radians(upper_foot.angle + 90)) * (upper_foot.size.y / 2) };
    upper_foot.angle = new_angle;
}

// SpotLight Method Definitions
SpotLight::SpotLight(const Transform &LightToWorld,
                     const MediumInterface &mediumInterface, const Spectrum &I,
                     Float totalWidth, Float falloffStart)
    : Light((int)LightFlags::DeltaPosition, LightToWorld, mediumInterface),
      pLight(LightToWorld(Point3f(0, 0, 0))),
      I(I),
      cosTotalWidth(std::cos(Radians(totalWidth))),
      cosFalloffStart(std::cos(Radians(falloffStart))) {}

SpotLight::SpotLight(const Transform& l2w, const RGB& intensity, Float width,
		Float fall) :
		Light(l2w) {
	mPos = lightToWorld(Point(0, 0, 0));
	mIntensity = intensity;
	mCosMaxWidth = cosf(Radians(width));
	mCosFall = cosf(Radians(fall));
}

/**********************************************************************************************************
*函 数 名: EarthFrameToBodyFrame
*功能说明: 转换向量到机体坐标系
*形    参: 转动角度 转动向量 转动后的向量指针
*返 回 值: 无
**********************************************************************************************************/
void EarthFrameToBodyFrame(Vector3f_t angle, Vector3f_t vector, Vector3f_t* vectorBf)
{
    Vector3f_t anglerad;

    anglerad.x = Radians(angle.x);
    anglerad.y = Radians(angle.y);
    anglerad.z = 0;
    *vectorBf  = VectorRotateToBodyFrame(vector, anglerad);
}

    void Camera::updateAngles()
    {
        if (std::abs(mFront.x) >= math::EPSILON
                || std::abs(mFront.z) >= math::EPSILON) {
            mXZAngle = Radians(std::atan2(mFront.x, mFront.z));
        }

        mYAngle = Radians(std::asin(mFront.y / mFront.length()));
    }

Transform RotateZ(float angle) {
    float sin_t = sinf(Radians(angle));
    float cos_t = cosf(Radians(angle));
    Matrix4x4 m(cos_t, -sin_t, 0, 0,
                sin_t,  cos_t, 0, 0,
                0,      0, 1, 0,
                0,      0, 0, 1);
    return Transform(m, Transpose(m));
}

// instead of lots of variables, pass latitude/longitude pairs
double Haversine( double dlatitude1, double dlongitude1, double dlatitude2, double dlongitude2, double radius, double* distance )
{
    double dLatitude = Radians(dlatitude2-dlatitude1);
    double dLongitude = Radians(dlongitude2-dlongitude1);
    double nA = pow ( sin(dLatitude/2.0), 2.0 ) + cos(Radians(dlatitude1)) * cos(Radians(dlatitude2)) * pow ( sin(dLongitude/2.0), 2.0 );
    double nC = 2.0 * atan2( sqrt(nA), sqrt( 1.0 - nA ));
    *distance = radius * nC;
    return *distance; 
}

//! renvoie la transformation associee a une rotation autour de l'axe Y, angle est en degres.
Transform RotateY( float angle )
{
    const float sin_t = sinf( Radians( angle ) );
    const float cos_t = cosf( Radians( angle ) );
    
    Matrix4x4 m(
         cos_t,   0, sin_t, 0,
             0,   1,     0, 0,
        -sin_t,   0, cos_t, 0,
             0,   0,     0, 1 );
    
    return Transform( m, m.transpose() );
}

void TestEnemy::update(Game* game) {
	Super::update(game);
	rad += Radians(2.0);

	auto bulletManager = game->getBulletManager();
	if (fireCount % 2 == 0 && frameCount > 60) {
		for (auto i : step(5)) {
			double fireRad = Radians(frameCount * 2) + TwoPi / 5 * i;
			auto bullet = std::make_shared<Bullet>(pos, Color(255, 100, 100), fireRad, 5.0, 0.0);
			bulletManager->add(bullet);
		}
	}
}