void DRay::pointTo( const DVector3& p )
{
if (p == mOrigin)
{
return;
}
DVector3 dir = p - mOrigin;
dir.normalize();
mDirection = dir;
}
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 ) ;
}
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;
}
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 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);
}
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();
}
}
}
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;
}
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 = §ors[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);
}
}
}
}
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;
}
// [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);
}
}
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();
}
}
}