Beispiel #1
0
	void DRay::pointTo( const DVector3& p )
	{
		if (p == mOrigin)
		{
			return;
		}
		DVector3 dir = p - mOrigin;
		dir.normalize();
		mDirection = dir;
	}
Beispiel #2
0
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 ) ;
}
Beispiel #3
0
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 printMsg(const qc_imu_imu_message& msg, const int i){
	std::cerr << "Message No= " << i << std::endl;
	std::cerr << "ACC= " << msg.accX << " " << msg.accY << " " << msg.accZ << std::endl;
	std::cerr << "MAG= " << msg.magX << " " << msg.magY << " " << msg.magZ << std::endl;
	std::cerr << "GYRO= " << msg.gyroX << " " << msg.gyroY << " " << msg.gyroZ << std::endl;
	Quaternion<double> q(msg.q0, msg.q1, msg.q2, msg.q3);
	DVector3 angles = q.toAngles();
	std::cerr << "R_P_Y= " << (angles.x()/M_PI)*180 << " " << (angles.y()/M_PI)*180 << " " << (angles.z()/M_PI)*180 << std::endl;
	std::cerr << "TS= " << msg.timestamp_sec + 1e-6 * msg.timestamp_usec << std::endl;
	std::cerr << std::endl;
	
}
Beispiel #5
0
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 ;
}
Beispiel #6
0
void P_SetSlope (secplane_t *plane, bool setCeil, int xyangi, int zangi, const DVector3 &pos)
{
	DAngle xyang;
	DAngle zang;

	if (zangi >= 180)
	{
		zang = 179.;
	}
	else if (zangi <= 0)
	{
		zang = 1.;
	}
	else
	{
		zang = (double)zangi;
	}
	if (setCeil)
	{
		zang += 180.;
	}

	xyang = (double)xyangi;

	DVector3 norm;

	if (ib_compatflags & BCOMPATF_SETSLOPEOVERFLOW)
	{
		// We have to consider an integer multiplication overflow here.
		norm[0] = FixedToFloat(FloatToFixed(zang.Cos()) * FloatToFixed(xyang.Cos()));
		norm[1] = FixedToFloat(FloatToFixed(zang.Cos()) * FloatToFixed(xyang.Sin()));
	}
	else
	{
		norm[0] = zang.Cos() * xyang.Cos();
		norm[1] = zang.Cos() * xyang.Sin();
	}
	norm[2] = zang.Sin();
	norm.MakeUnit();
	double dist = -norm[0] * pos.X - norm[1] * pos.Y - norm[2] * pos.Z;
	plane->set(norm[0], norm[1], norm[2], dist);
}
Beispiel #7
0
void P_Thing_SetVelocity(AActor *actor, const DVector3 &vec, bool add, bool setbob)
{
	if (actor != NULL)
	{
		if (!add)
		{
			actor->Vel.Zero();
			if (actor->player != NULL) actor->player->Vel.Zero();
		}
		actor->Vel += vec;
		if (setbob && actor->player != NULL)
		{
			actor->player->Vel += vec.XY();
		}
	}
}
Beispiel #8
0
	DRay DRay::getCommonPerpendicularTo( const DRay& b, DReal* length /* = NULL */)
	{
		// from http://www.cnblogs.com/chuzhouGIS/archive/2011/12/12/2284774.html
		if ( mDirection == DVector3::ZERO || b.mDirection == DVector3::ZERO)
		{
			return DRay(DVector3::ZERO, DVector3::ZERO);
		}
		DVector3 M, N;
		DVector3 A = mOrigin;
		DVector3	B = mOrigin + mDirection;
		DVector3	C = b.mOrigin;
		DVector3 D = b.mOrigin + b.mDirection;


		DReal	F1_ab = (B.x - A.x)*(B.x - A.x) + (B.y - A.y)*(B.y - A.y) + (B.z - A.z)*(B.z - A.z);
		DReal	F1_cd = (D.x - C.x)*(D.x - C.x) + (D.y - C.y)*(D.y - C.y) + (D.z - C.z)*(D.z - C.z);
		DReal	F2 = (B.x - A.x)*(D.x - C.x) + (B.y - A.y)*(D.y - C.y) + (B.z - A.z)*(D.z - C.z);
		DReal	F3_ab = (B.x - A.x)*(C.x - A.x) + (B.y - A.y)*(C.y - A.y) + (B.z - A.z)*(C.z - A.z);
		DReal	F3_cd = (D.x - C.x)*(C.x - C.x) + (D.y - C.y)*(C.y - C.y) + (D.z - C.z)*(C.z - C.z);
// 		F1(a,b)=[(Xb-Xa)*(Xb-Xa)+(Yb-Ya)*(Yb-Ya)+(Zb-Za)*(Zb-Za)]
// 
// 		F1(c,d)= [(Xd-Xc)*(Xd-Xc)+(Yd-Yc)*(Yd-Yc)+(Zd-Zc)*(Zd-Zc)]
// 
// 		F2()=[(Xb-Xa)*(Xd-Xc)+(Yb-Ya)*(Yd-Yc)+(Zb-Za)*(Zd-Zc)]
// 
// 		F3(a,b)=[(Xb-Xa)*(Xc-Xa)+(Yb-Ya)*(Yc-Ya)+(Zb-Za)*(Zc-Za)]
// 
// 		F3(c,d)=[(Xd-Xc)*(Xc-Xa)+(Yd-Yc)*(Yc-Ya)+(Zd-Zc)*(Zc-Za)]
		
		// two ray share the same plane.
		if ((F1_ab*F1_cd - F2*F2) == 0 || (F2*F2 - F1_ab*F1_cd))
		{
			return DRay(DVector3::ZERO, DVector3::ZERO);
		}

		M = A + (B-A)*(F3_ab*F1_cd - F3_cd*F2)/(F1_ab*F1_cd - F2*F2);
		N = C + (D-C)*(F3_cd*F1_ab -F2*F3_ab)/(F2*F2 - F1_ab*F1_cd);
		DVector3 dir = N-M;
		// two ray share the same plane.
		if (dir == DVector3::ZERO)
		{
			return DRay(DVector3::ZERO, DVector3::ZERO);
		}
		if (length != NULL)
		{
			*length = dir.length();
		}
		dir.normalize();
		return DRay(M, dir);
// 		由此得到两个垂足点的坐标:
//		t1=[F3(a,b)*F1(c,d)-F3(c,d)*F2()]/[F1(a,b)*F1(c,d)-F2()*F2()]
// 
// 		t2=[F3(c,d)*F1(a,b)-F2()*F3(a,b)]/[F2()*F2()-F1(a,b)*F1(c,d)]
// 
// 		M(Xm,Ym,Zm),
// 
// 		Xm=t1*(Xb-Xa)+Xa=(Xb-Xa)*[F3(a,b)*F1(c,d)-F3(c,d)*F2()]/[F1(a,b)*F1(c,d)-F2()*F2()]+Xa;
// 
// 		Ym=t1*(Yb-Ya)+Ya=(Yb-Ya)*[F3(a,b)*F1(c,d)-F3(c,d)*F2()]/[F1(a,b)*F1(c,d)-F2()*F2()]+Ya;
// 
// 		Zm=t1*(Zb-Za)+Za=(Zb-Za)*[F3(a,b)*F1(c,d)-F3(c,d)*F2()]/[F1(a,b)*F1(c,d)-F2()*F2()]+Za;
// 
// 
// 
// 		N(Xn,Yn,Zn),
// 
// 		Xn=t2*(Xd-Xc)+Xc=(Xd-Xc)*[F3(c,d)*F1(a,b)-F3(a,b)*F2()]/[F2()*F2()-F1(a,b)*F1(c,d)]+Xc;
// 
// 		Yn=t2*(Yd-Yc)+Yc=(Yd-Yc)*[F3(c,d)*F1(a,b)-F3(a,b)*F2()]/[F2()*F2()-F1(a,b)*F1(c,d)]+Yc;
// 
// 		Zn=t2*(Zd-Zc)+Zc=(Zd-Zc)*[F3(c,d)*F1(a,b)-F3(a,b)*F2()]/[F2()*F2()-F1(a,b)*F1(c,d)]+Zc;

	}
Beispiel #9
0
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)
				{
					// Only happens when all vertices in this sector are on the same line.
					// Let's just ignore this case.
					continue;
				}
				cross /= len;

				// Fix backward normals
				if ((cross.Z < 0 && j == 0) || (cross.Z > 0 && j == 1))
				{
					cross = -cross;
				}

				secplane_t *plane = j==0? &sec->floorplane : &sec->ceilingplane;

				double dist = -cross[0] * vertexes[vi3].fX() - cross[1] * vertexes[vi3].fY() - cross[2] * vt3.Z;
				plane->set(cross[0], cross[1], cross[2], dist);
			}
		}
	}
}
Beispiel #10
0
bool P_Thing_Projectile (int tid, AActor *source, int type, const char *type_name, DAngle angle,
	double speed, double vspeed, int dest, AActor *forcedest, int gravity, int newtid,
	bool leadTarget)
{
	int rtn = 0;
	PClassActor *kind;
	AActor *spot, *mobj, *targ = forcedest;
	FActorIterator iterator (tid);
	int defflags3;

	if (type_name == NULL)
	{
		kind = P_GetSpawnableType(type);
	}
	else
	{
		kind = PClass::FindActor(type_name);
	}
	if (kind == NULL)
	{
		return false;
	}

	// Handle decorate replacements.
	kind = kind->GetReplacement();

	defflags3 = GetDefaultByType(kind)->flags3;
	if ((defflags3 & MF3_ISMONSTER) && 
		((dmflags & DF_NO_MONSTERS) || (level.flags2 & LEVEL2_NOMONSTERS)))
		return false;

	if (tid == 0)
	{
		spot = source;
	}
	else
	{
		spot = iterator.Next();
	}
	while (spot != NULL)
	{
		FActorIterator tit (dest);

		if (dest == 0 || (targ = tit.Next()))
		{
			do
			{
				double z = spot->Z();
				if (defflags3 & MF3_FLOORHUGGER)
				{
					z = ONFLOORZ;
				}
				else if (defflags3 & MF3_CEILINGHUGGER)
				{
					z = ONCEILINGZ;
				}
				else if (z != ONFLOORZ)
				{
					z -= spot->Floorclip;
				}
				mobj = Spawn (kind, spot->PosAtZ(z), ALLOW_REPLACE);

				if (mobj)
				{
					mobj->tid = newtid;
					mobj->AddToHash ();
					P_PlaySpawnSound(mobj, spot);
					if (gravity)
					{
						mobj->flags &= ~MF_NOGRAVITY;
						if (!(mobj->flags3 & MF3_ISMONSTER) && gravity == 1)
						{
							mobj->Gravity = 1./8;
						}
					}
					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
					{
						mobj->Angles.Yaw = angle;
						mobj->VelFromAngle(speed);
						mobj->Vel.Z = vspeed;
					}
					// Set the missile's speed to reflect the speed it was spawned at.
					if (mobj->flags & MF_MISSILE)
					{
						mobj->Speed = mobj->VelToSpeed();
					}
					// Hugger missiles don't have any vertical velocity
					if (mobj->flags3 & (MF3_FLOORHUGGER|MF3_CEILINGHUGGER))
					{
						mobj->Vel.Z = 0;
					}
					if (mobj->flags & MF_SPECIAL)
					{
						mobj->flags |= MF_DROPPED;
					}
					if (mobj->flags & MF_MISSILE)
					{
						if (P_CheckMissileSpawn (mobj, spot->radius))
						{
							rtn = true;
						}
					}
					else if (!P_TestMobjLocation (mobj))
					{
						// If this is a monster, subtract it from the total monster
						// count, because it already added to it during spawning.
						mobj->ClearCounters();
						mobj->Destroy ();
					}
					else
					{
						// It spawned fine.
						rtn = 1;
					}
				}
			} while (dest != 0 && (targ = tit.Next()));
		}
		spot = iterator.Next();
	}

	return rtn != 0;
}
void qc_odometry_odometry_message_handler(MSG_INSTANCE mi, BYTE_ARRAY callData, void* clientData){
	(void) clientData; //no warning
	IPC_RETURN_TYPE err = IPC_OK;
	prev_omsg = omsg;
	err = IPC_unmarshallData(IPC_msgInstanceFormatter(mi), callData, &omsg, sizeof(omsg));
	if (newImuMessageAvailable){
		newImuMessageAvailable = false;
		prev_vmsg = vmsg;
		vmsg.timestamp_sec = omsg.timestamp_sec;
		vmsg.timestamp_usec = omsg.timestamp_usec;
		if (msgcounter > 2)
			validData = true;
		if (validData){
			double dtv = omsg.timestamp_sec - prev_omsg.timestamp_sec + 1e-6 * (omsg.timestamp_usec - prev_omsg.timestamp_usec);
			double dta = vmsg.timestamp_sec - prev_vmsg.timestamp_sec + 1e-6 * (vmsg.timestamp_usec - prev_vmsg.timestamp_usec);
			vmsg.x = omsg.x;
			vmsg.y = omsg.y;
			vmsg.z = omsg.z;
			vmsg.q0 = omsg.q0;
			vmsg.q1 = omsg.q1;
			vmsg.q2 = omsg.q2;
			vmsg.q3 = omsg.q3;
			vmsg.roll = omsg.roll;
			vmsg.pitch = omsg.pitch;
			vmsg.yaw = omsg.yaw;
			Transformation3<double> delta;
			Transformation3<double> p0(DVector3(prev_omsg.x, prev_omsg.y, prev_omsg.z), Quaternion<double>(prev_omsg.q0, prev_omsg.q1, prev_omsg.q2, prev_omsg.q3));
			Transformation3<double> p1(DVector3(omsg.x, omsg.y, omsg.z), Quaternion<double>(omsg.q0, omsg.q1, omsg.q2, omsg.q3));
			delta = p0.inv() * p1;
			///velocity && acceleration based on omsg
			if (dtv > 1e-7){
				dtv = 1./dtv;
				vmsg.vx = delta.translationVector.x() * dtv;
				vmsg.vy = delta.translationVector.y() * dtv;
				vmsg.vz = delta.translationVector.z() * dtv;
				if (dta > 1e-7){
					///take into account the different orientations of the velocities!
					p0.translationVector = DVector3(0,0,0);
					p0.rotationQuaternion = Quaternion<double>(prev_vmsg.q0, prev_vmsg.q1, prev_vmsg.q2, prev_vmsg.q3);
					p0.translationVector = p0 * DVector3(prev_vmsg.vx, prev_vmsg.vy, prev_vmsg.vz);
					
					p1.translationVector = DVector3(0,0,0);
					p1.rotationQuaternion = Quaternion<double>(vmsg.q0, vmsg.q1, vmsg.q2, vmsg.q3);
					p1.translationVector = p1 * DVector3(vmsg.vx, vmsg.vy, vmsg.vz);
					delta = p0.inv() * p1;
					dta = 1./dta;
					vmsg.avx = delta.translationVector.x() * dta;
					vmsg.avy = delta.translationVector.y() * dta;
					vmsg.avz = delta.translationVector.z() * dta;
				} else {
					vmsg.avx = vmsg.avy = vmsg.avz = 0;
				}
			} else {
				vmsg.avx = vmsg.avy = vmsg.avz = 0;
				vmsg.vx = vmsg.vy = vmsg.vz = 0;
			}
			
			///acceleration based on imu data
			///FIXME
			
			
			
			///acceleration based on tan roll and tan pitch
			Quaternion<double> q(imsg.q0, imsg.q1, imsg.q2, imsg.q3);
			DVector3 an = q.toAngles();
			vmsg.axtanp = tan(an.pitch()) * 9.81;
			vmsg.aytanr = tan(an.roll()) * 9.81;
			
			
			
			qc_odometry_publish_velocity_message(&vmsg);
		}
	}
	IPC_freeByteArray(callData);
	IPC_freeDataElements(IPC_msgInstanceFormatter(mi),&omsg);
	msgcounter++;
	if (!(msgcounter%10))
		std::cerr << "." << std::flush;
}
Beispiel #12
0
// [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);
	}
}
Beispiel #13
0
void InterceptDefaultAim(AActor *mobj, AActor *targ, DVector3 aim, double speed)
{
	if (mobj == nullptr || targ == nullptr)	return;
	mobj->Angles.Yaw = mobj->AngleTo(targ);
	mobj->Vel = aim.Resized(speed);
}
void AFastProjectile::Tick ()
{
	int i;
	DVector3 frac;
	int changexy;

	ClearInterpolation();
	double oldz = Z();

	if (!(flags5 & MF5_NOTIMEFREEZE))
	{
		//Added by MC: Freeze mode.
		if (bglobal.freeze || level.flags2 & LEVEL2_FROZEN)
		{
			return;
		}
	}


	// [RH] Ripping is a little different than it was in Hexen
	FCheckPosition tm(!!(flags2 & MF2_RIP));

	int count = 8;
	if (radius > 0)
	{
		while ( fabs(Vel.X) > radius * count || fabs(Vel.Y) > radius * count)
		{
			// we need to take smaller steps.
			count += count;
		}
	}

	// Handle movement
	if (!Vel.isZero() || (Z() != floorz))
	{
		// force some lateral movement so that collision detection works as intended.
		if ((flags & MF_MISSILE) && Vel.X == 0 && Vel.Y == 0 && !IsZeroDamage())
		{
			Vel.X = MinVel;
		}

		frac = Vel / count;
		changexy = frac.X != 0 || frac.Y != 0;
		int ripcount = count / 8;
		for (i = 0; i < count; i++)
		{
			if (changexy)
			{
				if (--ripcount <= 0)
				{
					tm.LastRipped.Clear();	// [RH] Do rip damage each step, like Hexen
				}
				
				if (!P_TryMove (this, Pos() + frac, true, NULL, tm))
				{ // Blocked move
					if (!(flags3 & MF3_SKYEXPLODE))
					{
						if (tm.ceilingline &&
							tm.ceilingline->backsector &&
							tm.ceilingline->backsector->GetTexture(sector_t::ceiling) == skyflatnum &&
							Z() >= tm.ceilingline->backsector->ceilingplane.ZatPoint(PosRelative(tm.ceilingline)))
						{
							// Hack to prevent missiles exploding against the sky.
							// Does not handle sky floors.
							Destroy ();
							return;
						}
						// [RH] Don't explode on horizon lines.
						if (BlockingLine != NULL && BlockingLine->special == Line_Horizon)
						{
							Destroy ();
							return;
						}
					}

					P_ExplodeMissile (this, BlockingLine, BlockingMobj);
					return;
				}
			}
			AddZ(frac.Z);
			UpdateWaterLevel ();
			oldz = Z();
			if (oldz <= floorz)
			{ // Hit the floor

				if (floorpic == skyflatnum && !(flags3 & MF3_SKYEXPLODE))
				{
					// [RH] Just remove the missile without exploding it
					//		if this is a sky floor.
					Destroy ();
					return;
				}

				SetZ(floorz);
				P_HitFloor (this);
				P_ExplodeMissile (this, NULL, NULL);
				return;
			}
			if (Top() > ceilingz)
			{ // Hit the ceiling

				if (ceilingpic == skyflatnum &&  !(flags3 & MF3_SKYEXPLODE))
				{
					Destroy ();
					return;
				}

				SetZ(ceilingz - Height);
				P_ExplodeMissile (this, NULL, NULL);
				return;
			}
			if (!frac.isZero() && ripcount <= 0) 
			{
				ripcount = count >> 3;
				Effect();
			}
		}
	}