C++ DVector3::Length方法代码示例

double GeodesicDist::ComputeAngleBetween2Lines(const DVector3 &v3PtCommon,
        const DVector3 &v3Pt1,
        const DVector3 &v3Pt2 )
{
    DVector3 P ;
    DVector3 Q ;

    DVector3Minus( v3Pt1, v3PtCommon, P ) ;
    DVector3Minus( v3Pt2, v3PtCommon, Q ) ;

    return acos( DVector3Dot( P, Q ) / ( P.Length() * Q.Length() ) ) ;
}

void GeodesicDist::ParameterizePt3ToPt2(const DVector3 &v3Origin,
                                        const DVector3 &v3OnePositivePt,
                                        const DVector3 &v3Pt,
                                        DVector2 &ptRes )
{
    DVector3 P;
    DVector3Minus( v3Pt, v3Origin, P ) ;

    DVector3 Q;
    DVector3Minus( v3OnePositivePt, v3Origin, Q ) ;

    double lengthQ = Q.Length() ;

    DVector3 PCrossQ ;
    DVector3Cross( P, Q, PCrossQ ) ;

    ptRes.x = DVector3Dot( P, Q ) / lengthQ ;
    ptRes.y = PCrossQ.Length() / lengthQ ;
}

void GeodesicDist::ComputePtOnLineWithDistance(const DVector3 &v3Pt1,
        const DVector3 &v3Pt2,
        const double &dDistanceAwayFromPt1,
        DVector3 &v3Result )
{
    DVector3 tmp ;

    DVector3Minus( v3Pt2, v3Pt1, tmp ) ;
    DVector3ScalarMul( tmp, 1/tmp.Length()*dDistanceAwayFromPt1 ) ;
    DVector3Add( tmp, v3Pt1, v3Result ) ;
}

static void P_SetSlopesFromVertexHeights(FMapThing *firstmt, FMapThing *lastmt, const int *oldvertextable)
{
	TMap<int, double> vt_heights[2];
	FMapThing *mt;
	bool vt_found = false;

	for (mt = firstmt; mt < lastmt; ++mt)
	{
		if (mt->info != NULL && mt->info->Type == NULL)
		{
			if (mt->info->Special == SMT_VertexFloorZ || mt->info->Special == SMT_VertexCeilingZ)
			{
				for (int i = 0; i < numvertexes; i++)
				{
					if (vertexes[i].fX() == mt->pos.X && vertexes[i].fY() == mt->pos.Y)
					{
						if (mt->info->Special == SMT_VertexFloorZ)
						{
							vt_heights[0][i] = mt->pos.Z;
						}
						else
						{
							vt_heights[1][i] = mt->pos.Z;
						}
						vt_found = true;
					}
				}
				mt->EdNum = 0;
			}
		}
	}

	for(int i = 0; i < numvertexdatas; i++)
	{
		int ii = oldvertextable == NULL ? i : oldvertextable[i];

		if (vertexdatas[i].flags & VERTEXFLAG_ZCeilingEnabled)
		{
			vt_heights[1][ii] = vertexdatas[i].zCeiling;
			vt_found = true;
		}

		if (vertexdatas[i].flags & VERTEXFLAG_ZFloorEnabled)
		{
			vt_heights[0][ii] = vertexdatas[i].zFloor;
			vt_found = true;
		}
	}

	// If vertexdata_t is ever extended for non-slope usage, this will obviously have to be deferred or removed.
	delete[] vertexdatas;
	vertexdatas = NULL;
	numvertexdatas = 0;

	if (vt_found)
	{
		for (int i = 0; i < numsectors; i++)
		{
			sector_t *sec = &sectors[i];
			if (sec->linecount != 3) continue;	// only works with triangular sectors

			DVector3 vt1, vt2, vt3, cross;
			DVector3 vec1, vec2;
			int vi1, vi2, vi3;

			vi1 = int(sec->lines[0]->v1 - vertexes);
			vi2 = int(sec->lines[0]->v2 - vertexes);
			vi3 = (sec->lines[1]->v1 == sec->lines[0]->v1 || sec->lines[1]->v1 == sec->lines[0]->v2)?
				int(sec->lines[1]->v2 - vertexes) : int(sec->lines[1]->v1 - vertexes);

			vt1 = DVector3(vertexes[vi1].fPos(), 0);
			vt2 = DVector3(vertexes[vi2].fPos(), 0);
			vt3 = DVector3(vertexes[vi3].fPos(), 0);

			for(int j=0; j<2; j++)
			{
				double *h1 = vt_heights[j].CheckKey(vi1);
				double *h2 = vt_heights[j].CheckKey(vi2);
				double *h3 = vt_heights[j].CheckKey(vi3);
				if (h1 == NULL && h2 == NULL && h3 == NULL) continue;

				vt1.Z = h1? *h1 : j==0? sec->GetPlaneTexZ(sector_t::floor) : sec->GetPlaneTexZ(sector_t::ceiling);
				vt2.Z = h2? *h2 : j==0? sec->GetPlaneTexZ(sector_t::floor) : sec->GetPlaneTexZ(sector_t::ceiling);
				vt3.Z = h3? *h3 : j==0? sec->GetPlaneTexZ(sector_t::floor) : sec->GetPlaneTexZ(sector_t::ceiling);

				if (P_PointOnLineSidePrecise(vertexes[vi3].fX(), vertexes[vi3].fY(), sec->lines[0]) == 0)
				{
					vec1 = vt2 - vt3;
					vec2 = vt1 - vt3;
				}
				else
				{
					vec1 = vt1 - vt3;
					vec2 = vt2 - vt3;
				}

				DVector3 cross = vec1 ^ vec2;

				double len = cross.Length();
				if (len == 0)
//.........这里部分代码省略.........

//.........这里部分代码省略.........
					}
					else
					{
						mobj->flags |= MF_NOGRAVITY;
					}
					mobj->target = spot;

					if (targ != NULL)
					{
						DVector3 aim = mobj->Vec3To(targ);
						aim.Z += targ->Height / 2;

						if (leadTarget && speed > 0 && !targ->Vel.isZero())
						{
							// Aiming at the target's position some time in the future
							// is basically just an application of the law of sines:
							//     a/sin(A) = b/sin(B)
							// Thanks to all those on the notgod phorum for helping me
							// with the math. I don't think I would have thought of using
							// trig alone had I been left to solve it by myself.

							DVector3 tvel = targ->Vel;
							if (!(targ->flags & MF_NOGRAVITY) && targ->waterlevel < 3)
							{ // If the target is subject to gravity and not underwater,
							  // assume that it isn't moving vertically. Thanks to gravity,
							  // even if we did consider the vertical component of the target's
							  // velocity, we would still miss more often than not.
								tvel.Z = 0.0;
								if (targ->Vel.X == 0 && targ->Vel.Y == 0)
								{
									goto nolead;
								}
							}
							double dist = aim.Length();
							double targspeed = tvel.Length();
							double ydotx = -aim | tvel;
							double a = g_acos (clamp (ydotx / targspeed / dist, -1.0, 1.0));
							double multiplier = double(pr_leadtarget.Random2())*0.1/255+1.1;
							double sinb = -clamp (targspeed*multiplier * g_sin(a) / speed, -1.0, 1.0);

							// Use the cross product of two of the triangle's sides to get a
							// rotation vector.
							DVector3 rv(tvel ^ aim);
							// The vector must be normalized.
							rv.MakeUnit();
							// Now combine the rotation vector with angle b to get a rotation matrix.
							DMatrix3x3 rm(rv, g_cos(g_asin(sinb)), sinb);
							// And multiply the original aim vector with the matrix to get a
							// new aim vector that leads the target.
							DVector3 aimvec = rm * aim;
							// And make the projectile follow that vector at the desired speed.
							mobj->Vel = aimvec * (speed / dist);
							mobj->AngleFromVel();
						}
						else
						{
nolead:
							mobj->Angles.Yaw = mobj->AngleTo(targ);
							mobj->Vel = aim.Resized (speed);
						}
						if (mobj->flags2 & MF2_SEEKERMISSILE)
						{
							mobj->tracer = targ;
						}
					}
					else

// [MC] Was part of P_Thing_Projectile, now its own function for use in ZScript.
// Aims mobj at targ based on speed and targ's velocity.
static void VelIntercept(AActor *targ, AActor *mobj, double speed, bool aimpitch = false, bool oldvel = false, bool leadtarget = true)
{
	if (targ == nullptr || mobj == nullptr)	return;

	DVector3 aim = mobj->Vec3To(targ);
	aim.Z += targ->Height / 2;

	if (leadtarget && speed > 0 && !targ->Vel.isZero())
	{
		// Aiming at the target's position some time in the future
		// is basically just an application of the law of sines:
		//     a/sin(A) = b/sin(B)
		// Thanks to all those on the notgod phorum for helping me
		// with the math. I don't think I would have thought of using
		// trig alone had I been left to solve it by myself.

		DVector3 tvel = targ->Vel;
		if (!(targ->flags & MF_NOGRAVITY) && targ->waterlevel < 3)
		{ // If the target is subject to gravity and not underwater,
		  // assume that it isn't moving vertically. Thanks to gravity,
		  // even if we did consider the vertical component of the target's
		  // velocity, we would still miss more often than not.
			tvel.Z = 0.0;

			if (targ->Vel.X == 0 && targ->Vel.Y == 0)
			{
				InterceptDefaultAim(mobj, targ, aim, speed);
				return;
			}
		}
		double dist = aim.Length();
		double targspeed = tvel.Length();
		double ydotx = -aim | tvel;
		double a = g_acos(clamp(ydotx / targspeed / dist, -1.0, 1.0));
		double multiplier = double(pr_leadtarget.Random2())*0.1 / 255 + 1.1;
		double sinb = -clamp(targspeed*multiplier * g_sin(a) / speed, -1.0, 1.0);
		DVector3 prevel = mobj->Vel;
		// Use the cross product of two of the triangle's sides to get a
		// rotation vector.
		DVector3 rv(tvel ^ aim);
		// The vector must be normalized.
		rv.MakeUnit();
		// Now combine the rotation vector with angle b to get a rotation matrix.
		DMatrix3x3 rm(rv, g_cos(g_asin(sinb)), sinb);
		// And multiply the original aim vector with the matrix to get a
		// new aim vector that leads the target.
		DVector3 aimvec = rm * aim;
		// And make the projectile follow that vector at the desired speed.
		mobj->Vel = aimvec * (speed / dist);
		mobj->AngleFromVel();
		if (oldvel)
		{
			mobj->Vel = prevel;
		}
		if (aimpitch) // [MC] Ripped right out of A_FaceMovementDirection
		{
			const DVector2 velocity = mobj->Vel.XY();
			mobj->Angles.Pitch = -VecToAngle(velocity.Length(), mobj->Vel.Z);
		}
	}
	else
	{
		InterceptDefaultAim(mobj, targ, aim, speed);
	}
}