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; }