void EnemyShip::changeVelocity(const float& dt, Engine::ParticleSystem& system) { if (target != nullptr) { Vector2 toTarget = target->getPosition() - shipPosition; if (LengthSquared(toTarget) != 0) { Vector2 normalizedtoTarget = PerpCCW(Normalized(toTarget)); shipRotation = atan2f(normalizedtoTarget.getY(), normalizedtoTarget.getX()); if (LengthSquared(toTarget) >= 0) { velocity = velocity + (Engine::Matrix2::rotation(shipRotation) * Engine::Vector2(0, -(acceleration * dt) * dt)); system.AddParticle(new THRUSTPARTICLEUP); } else { velocity = velocity + (Engine::Matrix2::rotation(shipRotation) * Engine::Vector2(0, (acceleration * dt) * dt)); system.AddParticle(new THRUSTPARTICLEDOWN); } } } }
SVector2 ArriveBehavior::Update(float deltaTime) { if (mActive) { SVector2 positionToDestination = mpAgent->GetDestination() - mpAgent->GetPosition(); SVector2 posToDestNorm = Normalize(positionToDestination); float maxSpeed = mpAgent->GetMaxSpeed(); SVector2 desiredVelocity = posToDestNorm * maxSpeed; float distSq = LengthSquared(positionToDestination); float velocitySq = LengthSquared(mpAgent->GetVelocity()); float slowingRadiusSq = 160000.0f; float stopRadiusSq = 25.0f; if (distSq < velocitySq) { if (distSq < slowingRadiusSq && distSq > stopRadiusSq ) { //desiredVelocity = -velocitySq/(2*sqrt(distSq)); desiredVelocity = posToDestNorm * maxSpeed * distSq/(slowingRadiusSq*slowingRadiusSq*slowingRadiusSq*slowingRadiusSq);// * maxSpeed/(distSq*distSq); } else if (distSq <= stopRadiusSq) { mpAgent->SetPosition(mpAgent->GetDestination()); desiredVelocity = SVector2(0.0f, 0.0f); } } return desiredVelocity - mpAgent->GetVelocity(); } return SVector2(0.0f, 0.0f); }
void ColourNormalFit::Permute3() { const Vec3 scale = Vec3( 1.0f / 0.5f); const Vec3 offset = Vec3(-1.0f * 0.5f); const Vec3 scalei = Vec3( 1.0f * 0.5f); // cache some values int const count = m_colours->GetCount(); Vec3 const* values = m_colours->GetPoints(); Scr3 const* freq = m_colours->GetWeights(); cQuantizer3<5,6,5> q = cQuantizer3<5,6,5>(); Scr3 berror = Scr3(DEVIANCE_MAXSUM); Vec3 c_start = m_start; Vec3 c_end = m_end; Scr3 l_start = LengthSquared(Normalize(scale * (offset + m_start))); Scr3 l_end = LengthSquared(Normalize(scale * (offset + m_end))); Vec3 q_start = Reciprocal(q.grid + Vec3(1.0f)); Vec3 q_end = q_start; // adjust offset towards sphere-boundary if (!(l_start < Scr3(1.0f))) q_start = Vec3(0.0f) - q_start; if (!(l_end < Scr3(1.0f))) q_end = Vec3(0.0f) - q_end; int trie = 0x3F; do { // permute end-points +-1 towards sphere-boundary Vec3 p_start = q_start & Vec3(!(trie & 0x01), !(trie & 0x02), !(trie & 0x04)); Vec3 p_end = q_end & Vec3(!(trie & 0x08), !(trie & 0x10), !(trie & 0x20)); p_start = q.SnapToLattice(c_start + p_start); p_end = q.SnapToLattice(c_end + p_end); // create a codebook // resolve "metric * (value - code)" to "metric * value - metric * code" Vec3 codes[3]; Codebook3n(codes, p_start, p_end); Scr3 merror = Scr3(DEVIANCE_BASE); for (int i = 0; i < count; ++i) { // find the closest code Vec3 value = Normalize(scale * (offset + values[i])); Scr3 dist; MinDeviance3<false>(dist, i, value, codes); // accumulate the error AddDeviance(dist, merror, freq[i]); } if (berror > merror) { berror = merror; m_start = p_start; m_end = p_end; } } while(--trie); }
bool Collisions::sphereToBox(RigidBody *sphere, RigidBody *box, CollisionInfo &info) { //translation vec2 nearest; vec2 clamp_box = box->collider.dims/2; vec2 sphere_in_box_world = sphere->position-box->position; nearest.x = clamp(sphere_in_box_world.x,-clamp_box.x,clamp_box.x); nearest.y = clamp(sphere_in_box_world.y,-clamp_box.y,clamp_box.y); float dist = LengthSquared(sphere_in_box_world-nearest); float radius = sphere->collider.radius*sphere->collider.radius; if(dist < radius) { float realDist = sqrt(dist); info.intersection = (box->position + nearest) + (sphere_in_box_world*(realDist-sphere->collider.radius)/realDist)/2; info.normal = Normalize(nearest - sphere_in_box_world); info.type = SPHERE_TO_BOX; // Side of box collision if(abs(nearest.x) < abs(nearest.y)) info.boxSideCol = SIDE_EDGE; else info.boxSideCol = UPPER_EGDE; return true; } else return false; }
float Vector::Length() { float r = (float)sqrt(LengthSquared()); if ( r < 0.0f ) r = -r; return r; }
float4 float4::normalize() const { f32 lsqr = LengthSquared(); if(NearZero(lsqr)) { return ZERO; }; f32 recip = InvSqrt(lsqr); return float4(vec[0]*recip, vec[1]*recip, vec[2]*recip, vec[3]*recip); };
bool Collides(const CollisionRadius& other) const { T radiusSquared = RadiusSquared() + other.RadiusSquared(); auto rawDistance = _position - other._position; return rawDistance.LengthSquared() <= radiusSquared; }
/*------------------------------------------------------------------------------ normalize this quaternion ------------------------------------------------------------------------------*/ Quaternion& Quaternion::Normalize() { float lengthSquared = LengthSquared(); float invLength = 0; if( lengthSquared ) { invLength = 1.0f / sqrtf( lengthSquared ); } *this *= invLength; return *this; }
Vector2 Vector2::NormalizeFast() const { float len = LengthSquared(); if (len > 0) return Mul(invsqrt(len)); return Vector2(); }
void Vector4::FastNormalize() { // No error checking, plus fast inverse square root float recLen = FastInvSqRt( LengthSquared() ); x *= recLen; y *= recLen; z *= recLen; w *= recLen; }
//-------------------------------------------------------------------- // Computes the average curvature per unit surface distance in the face //-------------------------------------------------------------------- float ComputeFaceCurvature(Point3 *n, Point3 *v, Point3 bc) { Point3 nc = (n[0]+n[1]+n[2])/3.0f; Point3 dn0 = n[0]-nc; Point3 dn1 = n[1]-nc; Point3 dn2 = n[2]-nc; Point3 c = (v[0] + v[1] + v[2]) /3.0f; Point3 v0 = v[0]-c; Point3 v1 = v[1]-c; Point3 v2 = v[2]-c; float d0 = DotProd(dn0,v0)/LengthSquared(v0); float d1 = DotProd(dn1,v1)/LengthSquared(v1); float d2 = DotProd(dn2,v2)/LengthSquared(v2); float ad0 = (float)fabs(d0); float ad1 = (float)fabs(d1); float ad2 = (float)fabs(d2); return (ad0>ad1)? (ad0>ad2?d0:d2): ad1>ad2?d1:d2; }
void PlayerTurret::rotateTurret() { Vector2 mousetoposition = Vector2((float)Input::GetMouseX(), (float)Input::GetMouseY()) - parentPosition; if (LengthSquared(mousetoposition) != 0) { rotationNormal = Engine::Normalized(mousetoposition); } }
float SSEVector3::Length() const { float result[ 4 ]; float lengthSquared = LengthSquared(); // Store in all floats, do not multiply fourth value: 0111 1111 const int mask = 0x7F; _mm_store_ss( result, _mm_sqrt_ss( _mm_dp_ps( vec, vec, mask ) ) ); return result[ 0 ]; }
Vector3 Vector3::Normalize(const Vector3& v) { float lengthSq = LengthSquared(v); if (lengthSq == 0.0f) { return v; } return v / static_cast<float>(Math::Sqrt(lengthSq)); }
void magnet::update() { if(!is_activated()) { return; } auto mag_pos = body_->GetWorldPoint(local_point_); auto ce = body_->GetContactList(); // std::set< b2Body* > processed; while(ce) { auto contact = ce->contact; if(contact->IsTouching()) { auto fixA = contact->GetFixtureA(); auto fixB = contact->GetFixtureB(); // b2Body* obj = nullptr; b2Fixture* fix = nullptr; if(fixA == sensor_) { // obj = fixB->GetBody(); fix = fixB; } else if(fixB == sensor_) { // obj = fixA->GetBody(); fix = fixA; } // if(obj != nullptr && processed.find(obj) == processed.end()) if(fix) { b2MassData md; fix->GetMassData(&md); auto metallic_mass = md.mass; // TODO: composition auto obj_pos = fix->GetBody()->GetWorldPoint(md.center); auto vec = mag_pos - obj_pos; auto sqr_dist = vec.LengthSquared(); auto magnitude = strength_ * metallic_mass / sqr_dist; vec *= magnitude / std::sqrt(sqr_dist); fix->GetBody()->ApplyForce(vec, obj_pos, true); body_->ApplyForce(-vec, mag_pos, true); // processed.insert(obj); } } ce = ce->next; } }
void UniformGrid::ClosestPoint(Point3 p, float radius, int &pindex, float &d) { xHitList.ClearAll(); yHitList.ClearAll(); zHitList.ClearAll(); hitList.SetCount(0); //find the cell in the XGrid TagCells(p,radius, 0); //find the cell in the YGrid TagCells(p,radius, 1); //find the cell in the ZGrid TagCells(p,radius, 2); BitArray usedList; usedList.SetSize(pointBase.Count()); usedList.ClearAll(); int closest = -1; d = 0.0f; Box3 localBounds; localBounds.Init(); localBounds += p; localBounds.EnlargeBy(radius); for (int i = 0; i < hitList.Count(); i++) { int index = hitList[i]; if (!usedList[index]) //check to see if we have processed this one or not { if (xHitList[index] && yHitList[index] && zHitList[index]) { usedList.Set(index); Point3 source = pointBase[index]; if (localBounds.Contains(source)) { float dist = LengthSquared(source-p); if ((dist < d) || (closest == -1)) { d = dist; closest = index; } } } } } pindex = closest; d = sqrt(d); }
static void FindVertexAngles(PatchMesh &pm, float *vang) { int i; for (i=0; i<pm.numVerts + pm.numVecs; i++) vang[i] = 0.0f; for (i=0; i<pm.numPatches; i++) { Patch &p = pm.patches[i]; for (int j=0; j<p.type; j++) { Point3 d1 = pm.vecs[p.vec[j*2]].p - pm.verts[p.v[j]].p; Point3 d2 = pm.vecs[p.vec[((j+p.type-1)%p.type)*2+1]].p - pm.verts[p.v[j]].p; float len = LengthSquared(d1); if (len == 0) continue; d1 /= Sqrt(len); len = LengthSquared (d2); if (len==0) continue; d2 /= Sqrt(len); float cs = DotProd (d1, d2); if (cs>=1) continue; // angle of 0 if (cs<=-1) vang[p.v[j]] += PI; else vang[p.v[j]] += (float) acos (cs); } } }
void RangeFit::Compress4( void* block ) { // cache some values int const count = m_colours->GetCount(); Vec3 const* values = m_colours->GetPoints(); // create a codebook Vec3 codes[4]; codes[0] = m_start; codes[1] = m_end; codes[2] = ( 2.0f/3.0f )*m_start + ( 1.0f/3.0f )*m_end; codes[3] = ( 1.0f/3.0f )*m_start + ( 2.0f/3.0f )*m_end; // match each point to the closest code u8 closest[16]; float error = 0.0f; for( int i = 0; i < count; ++i ) { // find the closest code float dist = FLT_MAX; int idx = 0; for( int j = 0; j < 4; ++j ) { float d = LengthSquared( m_metric*( values[i] - codes[j] ) ); if( d < dist ) { dist = d; idx = j; } } // save the index closest[i] = ( u8 )idx; // accumulate the error error += dist; } // save this scheme if it wins if( error < m_besterror ) { // remap the indices u8 indices[16]; m_colours->RemapIndices( closest, indices ); // save the block WriteColourBlock4( m_start, m_end, indices, block ); // save the error m_besterror = error; } }
static void FindVertexAngles (MNMesh &mm, float *vang) { int i; for (i=0; i<mm.numv; i++) vang[i] = 0.0f; for (i=0; i<mm.numf; i++) { int *vv = mm.f[i].vtx; int deg = mm.f[i].deg; for (int j=0; j<deg; j++) { Point3 d1 = mm.v[vv[(j+1)%deg]].p - mm.v[vv[j]].p; Point3 d2 = mm.v[vv[(j+deg-1)%deg]].p - mm.v[vv[j]].p; float len = LengthSquared(d1); if (len == 0) continue; d1 /= Sqrt(len); len = LengthSquared (d2); if (len==0) continue; d2 /= Sqrt(len); float cs = DotProd (d1, d2); // STEVE: What about angles over PI? if (cs>=1) continue; // angle of 0 if (cs<=-1) vang[vv[j]] += PI; else vang[vv[j]] += (float) acos (cs); } } }
static void Normalize(const gmVector3& a_vec, gmVector3& a_result) { float len2 = LengthSquared(a_vec); if(len2 != 0.0f) { float ooLen = 1.0f / (float)sqrt(len2); MulScalar(a_vec, ooLen, a_result); } else { a_result.m_x = 0.0f; a_result.m_y = 0.0f; a_result.m_z = 0.0f; } }
bool Collisions::sphereToSphere(RigidBody *sphere1, RigidBody *sphere2, CollisionInfo &info) { float dist = sphere1->collider.radius + sphere2->collider.radius; if(LengthSquared(sphere1->position - sphere2->position) < dist*dist) { vec2 pos1 = sphere1->position; vec2 pos2 = sphere2->position; info.intersection = vec2 {(pos1.x-pos2.x / 2), (pos1.y-pos2.y / 2)}; info.normal = Normalize(pos1 - pos2); info.type = SPHERE_TO_SPHERE; return true; } else return false; }
bool PolyOpWeldVertex::WeldShortPolyEdges (MNMesh & mesh, DWORD vertFlag) { // In order to collapse vertices, we turn them into edge selections, // where the edges are shorter than the weld threshold. bool canWeld = false; mesh.ClearEFlags (MN_USER); float threshSq = mThreshold*mThreshold; for (int i=0; i<mesh.nume; i++) { if (mesh.e[i].GetFlag (MN_DEAD)) continue; if (!mesh.v[mesh.e[i].v1].GetFlag (vertFlag)) continue; if (!mesh.v[mesh.e[i].v2].GetFlag (vertFlag)) continue; if (LengthSquared (mesh.P(mesh.e[i].v1) - mesh.P(mesh.e[i].v2)) > threshSq) continue; mesh.e[i].SetFlag (MN_USER); canWeld = true; } if (!canWeld) return false; return MNMeshCollapseEdges (mesh, MN_USER); }
// (This code was copied from EditPolyObj::EpfnCollapse.) bool VWeldMod::WeldShortPolyEdges (MNMesh & mesh, float thresh, DWORD flag) { // In order to collapse vertices, we turn them into edge selections, // where the edges are shorter than the weld threshold. bool canWeld = false; mesh.ClearEFlags (flag); float threshSq = thresh*thresh; for (int i=0; i<mesh.nume; i++) { if (mesh.e[i].GetFlag (MN_DEAD)) continue; if (!mesh.v[mesh.e[i].v1].GetFlag (flag)) continue; if (!mesh.v[mesh.e[i].v2].GetFlag (flag)) continue; if (LengthSquared (mesh.P(mesh.e[i].v1) - mesh.P(mesh.e[i].v2)) > threshSq) continue; mesh.e[i].SetFlag (flag); canWeld = true; } if (!canWeld) return false; MNMeshUtilities mmu(&mesh); return mmu.CollapseEdges (MN_USER); }
void CVector3::Normalize() { float lengthsq = LengthSquared(); //if the length is 0 if(::IsZero(lengthsq)) { //sets all elements to 0 Zero(); } else { //1 over the length squared float factor = (1.0f / sqrtf(lengthsq)); //Normalize x *= factor; y *= factor; z *= factor; } }
Optional<float> Ray::Intersects(const BoundingSphere& sphere) const { const auto toSphere = sphere.Center - this->Position; const auto toSphereLengthSquared = toSphere.LengthSquared(); const auto sphereRadiusSquared = sphere.Radius * sphere.Radius; if (toSphereLengthSquared < sphereRadiusSquared) { return 0; } const auto distance = Vector3::Dot(this->Direction, toSphere); if (distance < 0) { return Pomdog::NullOpt; } const auto discriminant = sphereRadiusSquared + distance * distance - toSphereLengthSquared; if (discriminant < 0) { return Pomdog::NullOpt; } return std::max(distance - std::sqrt(discriminant), 0.0f); }
//Set to unit vector void CVector3::Normalize() { float lengthsq = LengthSquared(); if(::IsZero(lengthsq)) { Zero(); } else { *this *= IvInvSqrt(lengthsq); /* The following is the expanded version of the above line: float factor = IvInvSqrt( lengthsq ); x *= factor; y *= factor; z *= factor; */ } }
float distPointLine( const vec2f &a, const vec2f &b, const vec2f &c, vec2f &p ) { vec2f ab = b-a; float lmag2 = LengthSquared(ab); float r = ((c.x - a.x)*(b.x-a.x) + (c.y-a.y)*(b.y-a.y)) / lmag2; if (r < 0.0f ) { p = a; return Length(a-c); } else if (r > 1.0f) { p = b; return Length(b-c); } else { p=a + r * ab; return Length( c-p ); } }
// If |projGridPt - GridPt| < gridCubeRadius // and probBitmap->GetPixel(src->UVW(bary)) < RandomZeroToOne() // Also a generic random factor. bool plDistributor::IProbablyDoIt(int iFace, Point3& del, const Point3& bary) const { if( fRand.RandZeroToOne() >= fOverallProb ) { return false; } if( (kIsoNone == fIsolation) || (kIsoLow == fIsolation) ) { if( LengthSquared(del) >= fSpacing*fSpacing ) { return false; } Point3 faceNorm = fSurfMesh->FaceNormal(iFace, FALSE); if( DotProd(del, faceNorm) < 0 ) { return false; } } if( IFailsAngProb(iFace, bary) ) { return false; } if( IFailsAltProb(iFace, bary) ) { return false; } if( IFailsProbBitmap(iFace, bary) ) { return false; } return true; }
double CVec3D::Length() const { return sqrt(LengthSquared()); }
inline const double Length() const { return sqrt(LengthSquared()); }