TraceResult IntersectSphere(SphereProperties &sphere, Ray &ray)
{
TraceResult traceResult;
Vector rayToSphereCenter = VectorSub( sphere.center, ray.origin);
float lengthRTSC2 = VectorDot( rayToSphereCenter, rayToSphereCenter ); // lengthRTSC2 = length of the ray from the ray's origin to the sphere's center squared
float closestApproach = VectorDot( rayToSphereCenter, ray.direction );
if (closestApproach < 0 ) // behind the ray origin
{
traceResult.hit = false;
return traceResult;
}
float halfCord2 = (sphere.radius * sphere.radius) - lengthRTSC2 + (closestApproach * closestApproach);
if(halfCord2 < 0) // no intersection
{
traceResult.hit = false;
return traceResult;
}
traceResult.hit = true;
traceResult.distance = closestApproach - sqrt(halfCord2);
return traceResult;
}
/* define behavior for bots in crusing state. This should be the flocking
* emergent behavioral model. It's what happens when enemies are just
* moving around the map trying to form groups */
void AICruisingEnter(ai_t *brain){
player_t *enemy;
vector_t destDir;
brain->prevState = brain->curState;
brain->curState = AI_CRUISING;
brain->timer = 0;
brain->timeout = AI_CRUISING_TIMEOUT + R_TIME_MOD*rand();
PlayerSetAnimation(brain->body, PLAYER_ANIM_RUN);
PlayerReset(brain->body);
/* I believe one might refer to this as cheating */
if((enemy = AIGetEnemy()) != NULL){
brain->destination.x = enemy->position.x + POS_ERR*UNIRAND(1.0);
brain->destination.z = enemy->position.z + POS_ERR*UNIRAND(1.0);
brain->destination.y = enemy->position.y;
/* set your body pointing in the right direction */
VectorSubtract(&destDir, &brain->destination, &brain->body->position);
VectorNormalize(&destDir); /* direction to our destination */
brain->destDir = destDir;
brain->dotDest = VectorDot(&destDir, &brain->body->forward);
brain->tPitch = 0;
}
}
bool LineQuadIntersect( const Vector3f vLineStart, const Vector3f vLineDir, const float fLength, Vector3f *vQuadVerts, const Vector3f vQuadNorm, Vector3f *vResult, float *fResultLength )
{
//float dotprod = VectorDot( vLineDir, vTriNorm );
//NOTE: Maybe remove Culling?
if( VectorDot( vLineDir, vQuadNorm ) > 0 ) //we are facing the back of our triangle
return false;
//Calc Distance to penetration point
//float t = -VectorDot( vTriNorm, vLineStart-vTriVerts[0] ) / VectorDot( vTriNorm, vLineDir );
float t = - ( vQuadNorm.x * (vLineStart.x - vQuadVerts->x) +
vQuadNorm.y * (vLineStart.y - vQuadVerts->y) + //more speedy version
vQuadNorm.z * (vLineStart.z - vQuadVerts->z) ) /
( vQuadNorm.x*vLineDir.x + vQuadNorm.y*vLineDir.y + vQuadNorm.z*vLineDir.z );
//Line started behind triangle or is too long?
if( t < 0 || t > fLength )
return false;
//Point to test:
Vector3f vPoint = vLineStart + vLineDir * t;
// Check if point is in triangle
if( PointInQuadSS( vQuadVerts, vPoint ) )
{
if( vResult )
*vResult = vPoint;
if( fResultLength )
*fResultLength = t;
return true;
}
else
return false;
}
//-----------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------
float CPlayerCar::CalculateMotorRpm( void )
{
//Get Ground contact for rear wheels
bool bGroundContact = m_pPhysVehicle->HasGroundContact(2) || m_pPhysVehicle->HasGroundContact(3);
bGroundContact = true;
if( !bGroundContact )
{
//motor accelerates by 4000 rpm per second if no ground contact
float desiredRpm = m_engineRpm + gpGlobals->frametime * 4000.0f;
return min( desiredRpm, m_info.motorMaxRpm );
}
//Wheels have ground contact.
//Get the linear forward velocity of the car
Vector3f vForward;
AngleToVector( GetAngle(), vForward );
float fForwardVel = VectorDot(GetVelocity(), -vForward);
float fWheelRpm = fForwardVel / (2.0f*PI*m_info.wheelRadius) * 60.0f;
float fAxleAndGearTranslation = m_info.axleRatio * m_info.gearRatios[m_engineGear-1];
float fMotorRpm = fWheelRpm * fAxleAndGearTranslation;
return fMotorRpm;
}
/* the angle between the normals of the two triangles should be less than 90-
deg, this compatibility test prevents two nearby triangles with disparate
orientation from entering the correspondence. */
int TriangleCorrsDict::NormCondition(__3dtree_Node *node)
{
dtVector norm1 =
__dt_CalculateTriangleUnitNorm(__lambda_model, node->id);
return (VectorDot(&__lambda_norm, &norm1) > 0);
}
void CalculateDirectionalLight(Vertex_VCN *pVertices, int num_vertices, Vector4 &light_direction, Vector4 &light_color)
{
for ( int i=0; i<num_vertices; i++ )
{
Vector4 normal = g_world_matrix.RotateVector(pVertices[i].m_Normal);
Vector4 intensity = VectorDot(normal, light_direction);
intensity.Abs();
pVertices[i].m_Color = intensity * light_color;
}
}
inline float CalculateLightingCoef(bool isShadowed, Vector &directionToLight, Vector &normal)
{
if( isShadowed )
// no light
return 0;
else
{ // how much light
float lightCoef = VectorDot(directionToLight, normal);
if (lightCoef < 0 ) lightCoef = 0;
return lightCoef;
}
}
//algorithm by http://www.blackpawn.com/texts/pointinpoly/default.html
//Does not work properly right now!
bool PointInTriangleBary( Vector3f *vTriVerts, Vector3f vTriNorm, Vector3f vPoint )
{
Vector3f v0 = vTriVerts[0];
Vector3f v1 = vTriVerts[1] - vTriVerts[0];
Vector3f v2 = vPoint - vTriVerts[0];
// Compute dot products
float dot00 = VectorDot(v0, v0);
float dot01 = VectorDot(v0, v1);
float dot02 = VectorDot(v0, v2);
float dot11 = VectorDot(v1, v1);
float dot12 = VectorDot(v1, v2);
// Compute barycentric coordinates
float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// Check if point is in triangle
return ( (u > 0) && (v > 0) && (u + v < 1.0f) );
}
void CalculatePointLight(Vertex_VCN *pVertices, int num_vertices, Vector4 &light_position, Vector4 &light_color)
{
for ( int i=0; i<num_vertices; i++ )
{
Vector4 position = pVertices[i].m_Position * g_world_matrix;
Vector4 vertex_to_light = light_position - position;
vertex_to_light.Normalize();
Vector4 normal = g_world_matrix.RotateVector(pVertices[i].m_Normal);
Vector4 intensity = VectorDot(normal, vertex_to_light);
intensity.Abs();
pVertices[i].m_Color = intensity * light_color;
}
}
void AIShotUpdate(ai_t *brain){
player_t *enemy;
float strength;
brain->timer++;
enemy = AIGetEnemy();
VectorSubtract(&brain->enemyDir, &enemy->position, &brain->body->position);
VectorNormalize(&brain->enemyDir);
brain->dotEnemy = VectorDot(&brain->body->forward, &brain->enemyDir);
strength = 1 - brain->dotEnemy;
brain->body->yaw +=
MAX_TURN*strength*(AIGetTurnDirectionH(brain, &brain->enemyDir));
brain->body->pitch +=
MAX_TURN*strength*(AIGetTurnDirectionV(brain, &brain->enemyDir));
PlayerSetForwardByAngle(brain->body, brain->body->pitch, brain->body->yaw);
}
void AIAttackUpdate(ai_t *brain){
float strength;
float diff;
static int count;
player_t *enemy;
vector_t prediction = {0.0, 0.0, 0.0};
vector_t temp;
projectile_t *proj;
brain->timer++;
/* use info you can see from the player to figure out where he'll be in
* the next step...we're trying to aim with some accuracy. The following
* calculations approximate the exact solution which can be found for
* the collision time of two moving projectiles. The general solution
* only exists under certain conditions. This always exists, and works
* quite well at reasonable range */
enemy = AIGetEnemy();
if(enemy->lFlag){
VectorSubtract(&prediction, &prediction, &enemy->right);
}
else if(enemy->rFlag){
VectorAdd(&prediction, &prediction, &enemy->right);
}
if(enemy->fFlag){
VectorAdd(&prediction, &prediction, &enemy->forward);
}
else if(enemy->bFlag){
VectorSubtract(&prediction, &prediction, &enemy->forward);
}
VectorSubtract(&temp, &enemy->position, &brain->body->position);
VectorAdd(&temp, &temp, &prediction);
VectorScale(&brain->enemyDir, &temp, enemy->speed);
brain->enemy_dis = VectorMagnitude(&brain->enemyDir);
VectorScale(&brain->enemyDir, &brain->enemyDir, 1.0/brain->enemy_dis);
brain->dotEnemy = VectorDot(&brain->enemyDir, &brain->body->forward);
/* decide if we need to move closer */
if(brain->enemy_dis > AI_DESIRED_RANGE){
diff = brain->enemy_dis - AI_DESIRED_RANGE + UNIRAND(3.0);
VectorScale(&temp, &brain->enemyDir, diff);
VectorAdd(&brain->destination, &temp, &brain->body->position);
AIUpdateForwardByDesire(brain, &brain->destination);
PlayerMoveForward(brain->body, brain->body->speed);
}
else{
if(brain->dotEnemy < 1){
strength = 2 - brain->dotEnemy;
/* we need to aim at our opponent pitch/yaw */
brain->body->yaw +=
MAX_TURN*strength*(AIGetTurnDirectionH(brain, &brain->enemyDir));
brain->body->pitch +=
MAX_TURN*strength*(AIGetTurnDirectionV(brain, &brain->enemyDir));
}
if(!(count % AI_ATTACK_PERIOD)){
/* fire the projectile in the exactly direction even though we may
* not have our bodies pointed there yet */
proj = ProjectileNew(&brain->enemyDir, &brain->body->position,
PROJ_SPEED, 2);
PListInsertAfter(plist, proj);
count = 0;
} count++;
}
}
void world_getPushBack(float boundingBox[3], vect3_t velocity, vect3_t pushback) {
float triangle[3][3] = {{0,0,0},{0,0,0},{0,0,0}},
matrix[16],
force;
int i, j, k, collided = 0, collisions = 0;
vect_t **triPtr = triangleList,
*tri;
vect3_t triangleNormal;
// Reset the pushback value
pushback[0] = 0;
pushback[1] = 0;
pushback[2] = 0;
// Setup the rotation matrix
setupRotationMatrix(matrix);
// setFlipMatrix(matrix);
for (i = 0; i < numTriangles; i++, triPtr++) {
// Copy triangle into correct format
// and translate by camera position
tri = *triPtr;
for (j = 0; j < 3; j++)
for (k = 0; k < 3; k++)
triangle[j][k] = tri[k + 3*j] - camera.position[k];
// Apply camera rotation to each vertex
for (k = 0; k < 3; k++)
rotatePoint(triangle[k], matrix);
// If if collides, get pushback vector
if (doesCollide(boundingBox, triangle)) {
collided = 1;
if (math_absF(pushback[1]) < math_absF(triangleNormal[1]))
pushback[1] = triangleNormal[1];
pushback[0] += triangleNormal[0];
pushback[2] += triangleNormal[2];
math_triangleNormal(&triangle[0][0], triangleNormal);
VectorDot(triangleNormal, velocity, force);
// printf("Collided with normal %6.2f %6.2f %6.2f\n",
// triangleNormal[0], triangleNormal[1], triangleNormal[2]);
if (force > 0) {
// printf("Force = %6.2f\n", force);
collisions++;
VectorScale(triangleNormal, -1);
// Scale the triangleNormal by the force
// to get the amount pushed back
// Set the maximum pushback amount
for (j = 0; j < 3; j++) {
if (math_absF(pushback[j]) < math_absF(triangleNormal[j]))
pushback[j] = triangleNormal[j];
}
}
}
}
// if (collided) {
// printf("Pushback = (%6.2f, %6.2f, %6.2f)\n",
// pushback[0], pushback[1], pushback[2]);
// }
}
//-----------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------
void CPlayerCar::Update( void )
{
m_bJustResetted = false;
//First check if we have to reset the car position
ResetIfNeeded();
//Calculate the new motor rpm
float rpm = CalculateMotorRpm();
if(rpm >= 0.0f)
{
if( rpm >= m_info.motorShiftUpRpm && m_engineGear < m_info.numberOfGears )
m_engineGear++; //shift up
else if( rpm < (USHORT)m_info.motorShiftDownRpm && m_engineGear > 1 )
m_engineGear--; //shift down
}
else
{
m_engineGear = 1;
}
m_engineRpm = (USHORT)abs(CalculateMotorRpm());
m_engineRpm = (USHORT)clamp<int>(m_engineRpm, (int)m_info.motorMinRpm, (int)m_info.motorMaxRpm);
float fPossibleWheelForce = CalculateMotorForceAtWheels( m_engineRpm );
//First calculate proper forward vector by multiplying the rotation matrix
//with the untransformed forward-vector.
Angle3d aCarDir = GetAngle();
Matrix3 mCarRotMat;
GetAngleMatrix3x3( aCarDir, mCarRotMat );
Vector3f vForward = mCarRotMat * Vector3f(0,0,1);
//update steering and gas
if( m_pPhysVehicle )
{
m_pPhysVehicle->UpdateSteering(m_fInputSteer);
float fVehSpeed = VectorDot( GetVelocity(), vForward );
if( fVehSpeed > 2.0f && VectorDot( vForward, GetVelocity() ) < 0.0f )
fVehSpeed = -fVehSpeed;
bool bMovingBackward = fVehSpeed < -2.0f;
bool bStandingStill = abs(fVehSpeed) <= 2.0f;
float fGas = 0.0f;
float fBreak = 0.0f;
if( bStandingStill )
fGas = m_fInputAccelerate;
else if( bMovingBackward )
{
if( m_fInputAccelerate > 0.0f )
fBreak = m_fInputAccelerate;
else
fGas = m_fInputAccelerate;
}
else
{
if( m_fInputAccelerate > 0.0f )
fGas = m_fInputAccelerate;
else
fBreak = -m_fInputAccelerate;
}
//Disable gas if under water
if(IsUnderWater())
fGas = 0.0f;
m_pPhysVehicle->UpdateGasBreak( fGas * fPossibleWheelForce, fBreak, m_fInputHandbrake );
//singletons::g_pEvtMgr->AddEventToQueue(
// new CCarMotorUpdateEvent( ev::CAR_MOTOR_UPDATE, this->GetIndex(), m_engineRpm, abs(fGas), m_engineGear ), RECIEVER_ID_ALL );
}
BaseClass::Update();
}
/**
* Solves a linear least squares problem to obtain a N degree polynomial that
* fits the specified input data as nearly as possible.
*
* Returns true if a solution is found, false otherwise.
*
* The input consists of two vectors of data points X and Y with indices 0..m-1
* along with a weight vector W of the same size.
*
* The output is a vector B with indices 0..n that describes a polynomial
* that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1]
* X[i] * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is
* minimized.
*
* Accordingly, the weight vector W should be initialized by the caller with the
* reciprocal square root of the variance of the error in each input data point.
* In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 /
* stddev(Y[i]).
* The weights express the relative importance of each data point. If the
* weights are* all 1, then the data points are considered to be of equal
* importance when fitting the polynomial. It is a good idea to choose weights
* that diminish the importance of data points that may have higher than usual
* error margins.
*
* Errors among data points are assumed to be independent. W is represented
* here as a vector although in the literature it is typically taken to be a
* diagonal matrix.
*
* That is to say, the function that generated the input data can be
* approximated by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n.
*
* The coefficient of determination (R^2) is also returned to describe the
* goodness of fit of the model for the given data. It is a value between 0
* and 1, where 1 indicates perfect correspondence.
*
* This function first expands the X vector to a m by n matrix A such that
* A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then
* multiplies it by w[i]./
*
* Then it calculates the QR decomposition of A yielding an m by m orthonormal
* matrix Q and an m by n upper triangular matrix R. Because R is upper
* triangular (lower part is all zeroes), we can simplify the decomposition into
* an m by n matrix Q1 and a n by n matrix R1 such that A = Q1 R1.
*
* Finally we solve the system of linear equations given by
* R1 B = (Qtranspose W Y) to find B.
*
* For efficiency, we lay out A and Q column-wise in memory because we
* frequently operate on the column vectors. Conversely, we lay out R row-wise.
*
* http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares
* http://en.wikipedia.org/wiki/Gram-Schmidt
*/
static bool SolveLeastSquares(const float* x,
const float* y,
const float* w,
uint32_t m,
uint32_t n,
float* out_b,
float* out_det) {
// MSVC does not support variable-length arrays (used by the original Android
// implementation of this function).
#if defined(COMPILER_MSVC)
const uint32_t M_ARRAY_LENGTH =
LeastSquaresVelocityTrackerStrategy::kHistorySize;
const uint32_t N_ARRAY_LENGTH = Estimator::kMaxDegree;
DCHECK_LE(m, M_ARRAY_LENGTH);
DCHECK_LE(n, N_ARRAY_LENGTH);
#else
const uint32_t M_ARRAY_LENGTH = m;
const uint32_t N_ARRAY_LENGTH = n;
#endif
// Expand the X vector to a matrix A, pre-multiplied by the weights.
float a[N_ARRAY_LENGTH][M_ARRAY_LENGTH]; // column-major order
for (uint32_t h = 0; h < m; h++) {
a[0][h] = w[h];
for (uint32_t i = 1; i < n; i++) {
a[i][h] = a[i - 1][h] * x[h];
}
}
// Apply the Gram-Schmidt process to A to obtain its QR decomposition.
// Orthonormal basis, column-major order.
float q[N_ARRAY_LENGTH][M_ARRAY_LENGTH];
// Upper triangular matrix, row-major order.
float r[N_ARRAY_LENGTH][N_ARRAY_LENGTH];
for (uint32_t j = 0; j < n; j++) {
for (uint32_t h = 0; h < m; h++) {
q[j][h] = a[j][h];
}
for (uint32_t i = 0; i < j; i++) {
float dot = VectorDot(&q[j][0], &q[i][0], m);
for (uint32_t h = 0; h < m; h++) {
q[j][h] -= dot * q[i][h];
}
}
float norm = VectorNorm(&q[j][0], m);
if (norm < 0.000001f) {
// vectors are linearly dependent or zero so no solution
return false;
}
float invNorm = 1.0f / norm;
for (uint32_t h = 0; h < m; h++) {
q[j][h] *= invNorm;
}
for (uint32_t i = 0; i < n; i++) {
r[j][i] = i < j ? 0 : VectorDot(&q[j][0], &a[i][0], m);
}
}
// Solve R B = Qt W Y to find B. This is easy because R is upper triangular.
// We just work from bottom-right to top-left calculating B's coefficients.
float wy[M_ARRAY_LENGTH];
for (uint32_t h = 0; h < m; h++) {
wy[h] = y[h] * w[h];
}
for (uint32_t i = n; i-- != 0;) {
out_b[i] = VectorDot(&q[i][0], wy, m);
for (uint32_t j = n - 1; j > i; j--) {
out_b[i] -= r[i][j] * out_b[j];
}
out_b[i] /= r[i][i];
}
// Calculate the coefficient of determination as 1 - (SSerr / SStot) where
// SSerr is the residual sum of squares (variance of the error),
// and SStot is the total sum of squares (variance of the data) where each
// has been weighted.
float ymean = 0;
for (uint32_t h = 0; h < m; h++) {
ymean += y[h];
}
ymean /= m;
float sserr = 0;
float sstot = 0;
for (uint32_t h = 0; h < m; h++) {
float err = y[h] - out_b[0];
float term = 1;
for (uint32_t i = 1; i < n; i++) {
term *= x[h];
err -= term * out_b[i];
}
sserr += w[h] * w[h] * err * err;
float var = y[h] - ymean;
sstot += w[h] * w[h] * var * var;
}
*out_det = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
return true;
}
