bool Sphere::intersect(Ray ray, float* t, vec3f* intersectionPoint) const noexcept
{
    const vec3f k{implementation->position - ray.position};

    const float a = ray.direction.dot(k);
    const float D = a * a - (k.dot(k) - implementation->radius * implementation->radius);

    if (D < 0.0F)
        return false;

    const float sqrtD = std::sqrt(D);
    const float t1 = a - sqrtD;
    const float t2 = a + sqrtD;

    const float minT = std::min(t1, t2);
    const float maxT = std::max(t1, t2);

    const float fineT = minT >= 0.0F ? minT : maxT;

    if (fineT < 0.0F)
        return false;

    if (t) *t = fineT;

    if (intersectionPoint)
        *intersectionPoint = ray.position + ray.direction * fineT;

    return true;
}

vec3f vec3f::random(){
	static vec3f rnd;
	rnd.x=random_double()*2-1;
	rnd.y=random_double()*2-1;
	rnd.z=random_double()*2-1;
	rnd.normalize();
	return rnd;
}

bool Camera::InView(const vec3f& p, float radius) const {
	const vec3f t = (p - pos);

	return
		(t.dot3D(frustumR) < radius) &&
		(t.dot3D(frustumL) < radius) &&
		(t.dot3D(frustumB) < radius) &&
		(t.dot3D(frustumT) < radius);
}

 camera()
 {      
     V.make_identity();
     P.make_identity();
     R.make_identity();               
     position.set_value( vec3f(0,0,100) );
     euler_rotation.set_value( vec3f(0,0,0) );
     rotation.set_value( vec3f(0,1,0), 0);
 }

                // Given a point and a plane (defined by a coplanar point and a normal), compute the closest point
                // in the plane.  (The plane is unbounded.)
                vec3f NearestPointInPlane(const vec3f &point, const vec3f &planePoint, const vec3f &planeNormal)
                {
                        vec3f nearestPoint;
                        vec3f pointDelta    = point - planePoint;

                        float delta = planeNormal.dot(pointDelta) / planeNormal.dot(planeNormal);
                        nearestPoint = point - delta*planeNormal;
                        return nearestPoint;
                }

color texture2D(vec3f coords, texture t){
	int x = coords.x() * t.width;
	int y = t.height-(coords.y() * t.height);
	if(t.bytesperpixel == 4){
		return t.data[y*t.height + x];
	}
	uint8_t* data = (uint8_t*)t.data;
	
	int col= (int)(data[t.bytesperpixel*(y*t.height + x)]);
	
	return gammaCorrect((color){static_cast<uint8_t>(col>>24),static_cast<uint8_t>((col>>16) &255),static_cast<uint8_t>((col>>8)&255),255});

/**
* LookTowards
*
* Note 1: Function will exit if front_vec is (close to) parallel to the Y-axis; supply your own up_vec if this is the case.
* Note 2: Not fully tested, use at own risk...
*
* Example:
*	Make camera look at 'my_node'. Note that world space positions are used.
*	camera->lookAt( my_node->getWorldPosition() - camera->getWorldPosition() );
*/
void Transformable::lookTowards(
		vec3f	front_vec,
		vec3f	up_vec
	)
{
	vec3f right;
	vec3f up;
	vec3f prev_up;

	front_vec.normalize();
	up_vec.normalize();

	if (abs(up_vec.dot(front_vec)) > 0.99999f) {
		return;
	}

	if (parent && parent->is_transformable) {
		mat3f mat;
		R = ((Transformable *) parent)->getWorldMatrix().rotationMatrix();
		R.inv();

		prev_up = up_vec;

		right = front_vec.cross(prev_up);
		up = right.cross(front_vec);

		right.normalize();
		up.normalize();

		mat.setCol(0, right);
		mat.setCol(1, up);
		mat.setCol(2, -front_vec);

		R = R * mat;

	} else {
		prev_up = up_vec;

		right = front_vec.cross(prev_up);
		up = right.cross(front_vec);

		right.normalize();
		up.normalize();

		R.setCol(0, right);
		R.setCol(1, up);
		R.setCol(2, -front_vec);
	}
}

void Logstalgia::addGroup(std::string grouptitle, std::string groupregex, int percent, vec3f colour) {

    if(percent<0) return;

    int remainpc = (int) ( ((float) remaining_space/total_space) * 100);

    if(percent==0) {
        percent=remainpc;
    }

    if(remainpc<percent) return;

    int space = (int) ( ((float)percent/100) * total_space );

    int top_gap    = total_space - remaining_space;
    int bottom_gap = display.height - (total_space - remaining_space + space);

    //debugLog("group %s: regex = %s, remainpc = %d, space = %d, top_gap = %d, bottom_gap = %d\n",
    //    grouptitle.c_str(), groupregex.c_str(), remainpc, space, top_gap, bottom_gap);

    Summarizer* summ = new Summarizer(fontSmall, paddle_x, top_gap, bottom_gap, update_rate, groupregex, grouptitle);
//    summ->showCount(true);

    if(colour.length2() > 0.01f) {
        summ->setColour(colour);
    }

    summGroups.push_back(summ);

    remaining_space -= space;
}

double SynthScore::handOrientationScore(const DisembodiedObject &object, const mat4f &modelToWorld, const Agent &agent)
{
    const vec3f diff = modelToWorld * (object.model->bbox.getCenter() + vec3f(object.agentFace.x, object.agentFace.y, 0.0f)) - modelToWorld * object.model->bbox.getCenter();

    //const OBBf worldBBox = modelToWorld * OBBf(object.model->bbox);
    //vec3f agentFaceDir = (worldBBox.getCenter() - agent.handPos());
    //agentFaceDir.z = 0.0f;
    //float dot = diff.getNormalized() | agentFaceDir.getNormalized();

    float dot = diff.getNormalized() | agent.gazeDir;

    if (dot < 0.0f)
        return 0.0f;

    return dot;
}

void matrix4x4f::rotate( const float &angle, vec3f &axis )
{
    float s = sin(DEGTORAD(angle));
    float c = cos(DEGTORAD(angle));

    axis.norm();

    float ux = axis.x;
    float uy = axis.y;
    float uz = axis.z;
    
    m[0]  = c + (1-c) * ux*ux;
    m[1]  = (1-c) * ux*uy + s*uz;
    m[2]  = (1-c) * ux*uz - s*uy;
    m[3]  = 0;
    
    m[4]  = (1-c) * uy*ux - s*uz;
    m[5]  = c + (1-c) * pow(uy,2);
    m[6]  = (1-c) * uy*uz + s*ux;
    m[7]  = 0;
    
    m[8]  = (1-c) * uz*ux + s*uy;
    m[9]  = (1-c) * uz*uy - s*ux;
    m[10] = c + (1-c) * pow(uz,2);
    m[11] = 0;
    
    m[12] = 0;
    m[13] = 0;
    m[14] = 0;
    m[15] = 1;
}

bool Camera::AABBInOriginPlane(const vec3f& plane, const vec3f& mins, const vec3f& maxs) const {
	vec3f fp;
		fp.x = (plane.x > 0.0f)? mins.x: maxs.x;
		fp.y = (plane.y > 0.0f)? mins.y: maxs.y;
		fp.z = (plane.z > 0.0f)? mins.z: maxs.z;
	return (plane.dot3D(fp - pos) < 0.0f);
}

particle_intermediate make_particle_jet(int num, vec3f start, vec3f dir, float len, float angle, vec3f col, float speedmod)
{
    std::vector<cl_float4> p1;
    std::vector<cl_float4> p2;
    std::vector<cl_uint> colours;

    for(uint32_t i = 0; i<num; i++)
    {
        float len_pos = randf_s(0, len);

        float len_frac = len_pos / len;

        vec3f euler = dir.get_euler();

        euler = euler + (vec3f){0, randf_s(-angle, angle), randf_s(-angle, angle)};

        vec3f rot_pos = (vec3f){0.f, len_pos, 0.f}.rot({0.f, 0.f, 0.f}, euler);

        vec3f final_pos = start + rot_pos;

        final_pos = final_pos + randf<3, float>(-len/40.f, len/40.f);


        float mod = speedmod;

        p1.push_back({final_pos.v[0], final_pos.v[1], final_pos.v[2]});

        vec3f pos_2 = final_pos * mod;

        //p2.push_back({pos_2.v[0], pos_2.v[1], pos_2.v[2]});

        col = clamp(col, 0.f, 1.f);

        colours.push_back(rgba_to_uint(col));
    }

//******************************************************************
//FUNCTION:
void COutdoorLightScattering::__getRaySphereIntersection(vec3f vRayOrigin, vec3f vRayDirection, vec3f vSphereCenter, float vSphereRadius, vec2f& voIntersection)
{
	vRayOrigin -= vSphereCenter;

	float A = vRayDirection.dot(vRayDirection);
	float B = 2 * vRayOrigin.dot(vRayDirection);
	float C = vRayOrigin.dot(vRayOrigin) - vSphereRadius * vSphereRadius;
	float D = B * B - 4 * A * C;

	if (D < 0)
	{
		voIntersection = vec2f(-1);
	}
	else
	{
		D = sqrt(D);
		voIntersection = vec2f((-B - D) / 2 * A, (-B + D) / 2 * A);
	}
}

vec3f interpolate(vec3f current, vec3f target, float timeDelta, float maxSpeed, float maxDistance)
{
	float distance = current.getDistanceFrom(target);
	if(distance > maxDistance)
		return target;
	else {
		float d = std::min(distance, (distance/maxDistance)*maxSpeed*timeDelta);
		return current + (target-current).normalize()*d;
	}
}

vec3f::vec3f(const vec3f& aVec)
{
  float v[3];

  aVec.get(v[0], v[1], v[2]);

  vec[X] = v[X];
  vec[Y] = v[Y];
  vec[Z] = v[Z];
}

void CSMFRenderer::DrawPotentiallyVisibleSquares(const Camera* cam, int, int) {
	const CReadMap* rm = readMap;
	const float* hm = rm->GetHeightmap();
	// const vec3f* nm = &rm->facenormals[0];

	// each square is drawn clockwise topside-up
	glEnable(GL_CULL_FACE);
	glCullFace(GL_BACK);
	glFrontFace(GL_CW);

	// our normals are always unit-length
	glDisable(GL_NORMALIZE);
	glPolygonMode(GL_FRONT, GL_FILL);

	for (int i = 0; i < numSquares; i++) {
		Square& q = squares[i];

		if (!SquareRowVisible(q.idx, rm, cam)) {
			// advance to start of next row
			i += (numSquaresX - (i % numSquaresX));
			continue;
		}

		// note: slightly inefficient, would be better to use y-extremes of <q>
		vec3f minBounds = q.GetMinBounds(); minBounds.y = rm->currMinHeight;
		vec3f maxBounds = q.GetMaxBounds(); maxBounds.y = rm->currMaxHeight;
		const vec3f v = (cam->pos - q.mid);
		const float dSq = v.sqLen3D();
		const bool draw = (dSq < viewRadiusSq && cam->InView(minBounds, maxBounds));

		if (draw) {
			DrawSquare(q, v, rm, hm);
		}
	}

	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glEnable(GL_NORMALIZE);

	glDisable(GL_CULL_FACE);
	glFrontFace(GL_CCW);
}

bool BBox::Overlap( const vec3f& p ) const
{
    bool x = (pMax.x() >= p.x()) && (pMin.x() <= p.x());
    bool y = (pMax.y() >= p.y()) && (pMin.y() <= p.y());
    bool z = (pMax.z() >= p.z()) && (pMin.z() <= p.z());
    return (x && y && z);
}

bool Entity::accumulateForce(vec3f steeringToAdd)
{
    float forceUsed = mSteeringForce.getLength();
    float forceRemaining = mMaxForce - forceUsed;
    
    if (forceRemaining <= 0)
        return false;
        
    float forceToAdd = steeringToAdd.getLength();
    
    if (forceToAdd < forceRemaining)
    {
        mSteeringForce += steeringToAdd;
    }
    else
    {
        steeringToAdd.normalize();
        mSteeringForce += steeringToAdd * forceRemaining;
    }
    
    return true;
}

vec2f Grid::coordsToMapPos(vec3f coords) {
	GLfloat width = d_cellWidth * d_rows;
	GLfloat height = d_cellHeight * d_cols;
	
	GLfloat x = floor(coords.x() / width);
	GLfloat y = floor(coords.z() / height);
	
	// Clamp the values into the bounds
	if (x > d_rows) {
		x = d_rows;
	} else if (x < 0) {
		x = 0;
	}
	
	if (y > d_cols) {
		y = d_cols;
	} else if (y < 0) {
		y = 0;
	}
	
	return vec2f(x, y);
}

	static void Spawn (CPlayerEntity *Player, vec3f Start, vec3f AimDir, int DamageMultiplier, int Speed)
	{
		vec3f	dir = AimDir.ToAngles();

		anglef angles = dir.ToVectors ();
		
		CProx	*Prox = QNewEntityOf CProx;
		Prox->State.GetOrigin() = Start;
		Prox->Velocity =	(AimDir * Speed)
							.MultiplyAngles (200 + crand() * 10.0f, angles.Up)
							.MultiplyAngles (crand() * 10.0f, angles.Right);
		Prox->State.GetAngles() = dir - vec3f(90, 0, 0);
		Prox->PhysicsType = PHYSICS_BOUNCE;
		Prox->GetSolid() = SOLID_BBOX; 
		Prox->State.GetEffects() |= FX_GRENADE;
		Prox->GetClipmask() = CONTENTS_MASK_SHOT|CONTENTS_LAVA|CONTENTS_SLIME;
		Prox->State.GetRenderEffects() |= RF_IR_VISIBLE;
		
		Prox->GetMins().Set (-6, -6, -6);
		Prox->GetMaxs().Set (6, 6, 6);

		Prox->State.GetModelIndex() = ModelIndex ("models/weapons/g_prox/tris.md2");
		Prox->SetOwner(Player);
		Prox->Firer = Player;
		Prox->Touchable = true;
		Prox->ThinkType = PROXTHINK_EXPLODE;
		Prox->Damage = PROX_DAMAGE * DamageMultiplier;
		Prox->ClassName = "prox";

		switch (DamageMultiplier)
		{
		case 1:
			Prox->NextThink = Level.Frame + PROX_TIME_TO_LIVE;
			break;
		case 2:
			Prox->NextThink = Level.Frame + 300;
			break;
		case 4:
			Prox->NextThink = Level.Frame + 150;
			break;
		case 8:
			Prox->NextThink = Level.Frame + 100;
			break;
		default:
			Prox->NextThink = Level.Frame + PROX_TIME_TO_LIVE;
			break;
		}

		Prox->Link ();
	}