#include "munga.h" #pragma hdrstop #include "mover.h" #include "player.h" #include "boxsolid.h" #include "interest.h" #include "collasst.h" #include "doorfram.h" #include "door.h" #include "line.h" #include "app.h" #include "notation.h" // // The blend fraction the last dead-reckoned step used, and whether it // blended at all rather than snapping. Only read by the RP412CAMLOG trace // in Mover::DeadReckon, which needs them from the branch that computes // them a few lines earlier. // static Logical gLastLerpUsed = False; static Scalar gLastPercent = 0.0f; // // The one replicant the RP412CAMLOG traces describe. Latched here because // the renderer reports on the same entity from the other end - what its // motion looks like on screen - and two traces about two different pods // would compare nothing. // static EntityID gTracedEntity = EntityID::Null; static Logical gTracedLatched = False; EntityID MoverTracedEntity() { return gTracedEntity; } // // Prediction-error totals for the RP412CAMLOG trace. Shared across // replicants deliberately: the question - does constant-velocity // extrapolation hold over one send interval - is about the model, not // about any one pod, so a whole grid contributing samples is a better // answer rather than a muddled one. // static int gPredictSamples = 0; static Scalar gPredictAlong = 0.0f; static Scalar gPredictAlongAbs = 0.0f; static Scalar gPredictAcross = 0.0f; static Scalar gPredictMilliseconds = 0.0f; static Scalar gPredictNextSay = 0.0f; // // Bounds on the replication interval estimate, in seconds. // // The first pair decide what is allowed into the sample window at all: a // non-positive gap is a duplicate or a reordered packet, and a multi-second // one is a join, a pause or a stall. Neither says anything about the rate // the sender is actually keeping. // // The second pair are a backstop on the answer, set deliberately wide so // that in every sane case the median decides it and these never bind. // // Measured send rate on a live connection is about 30ms, so half a second // is already sixteen times slower than anything healthy. // static const Scalar kMinimumUpdateInterval = 0.001f; static const Scalar kOutlierUpdateInterval = 0.5f; static const Scalar kMinimumPredictedInterval = 0.010f; // // Never predict further ahead than this, which puts a floor under the // dead reckoner's blend fraction: at a 20ms step the worst case becomes // 0.02/(0.25+0.02), near enough 7% of the gap per step, so a pod still // converges on its projection in a dozen steps instead of crawling. // static const Scalar kMaximumPredictedInterval = 0.25f; // // A gap this long is a stall, not jitter - six times the observed rate. // A gap this short cannot be a sender keeping to 30ms, so it is a packet // that was already waiting when we finally got round to reading it. // static const Scalar kLongGapThreshold = 0.200f; static const Scalar kQueuedGapThreshold = 0.005f; //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // // RP412NETPREDICT=0 restores the original single-sample prediction, so the // two can be compared on the same build and the same connection. // //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // // Every path out of PredictUpdateInterval goes through here. It used not // to, and the one that skipped it was the bug. // static Scalar ClampPredictedInterval(Scalar interval) { if (interval < kMinimumPredictedInterval) { return kMinimumPredictedInterval; } if (interval > kMaximumPredictedInterval) { return kMaximumPredictedInterval; } return interval; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // static Logical UseMedianPrediction() { static int cached = -1; if (cached < 0) { const char *setting = getenv("RP412NETPREDICT"); cached = (setting && *setting == '0') ? 0 : 1; } return cached ? True : False; } //############################################################################# //############################### Mover ################################# //############################################################################# //############################################################################# // Shared Data Support // Derivation* Mover::GetClassDerivations() { static Derivation classDerivations(Entity::GetClassDerivations(), "Mover"); return &classDerivations; } Mover::SharedData Mover::DefaultData( Mover::GetClassDerivations(), Mover::GetMessageHandlers(), Mover::GetAttributeIndex(), Mover::StateCount, (Entity::MakeHandler)Mover::Make ); //############################################################################# // Message Support // #if 0 const Receiver::HandlerEntry Mover::MessageHandlerEntries[]= { MESSAGE_ENTRY(Mover, Update) }; Entity::MessageHandlerSet Mover::MessageHandlers( ELEMENTS(Mover::MessageHandlerEntries), Mover::MessageHandlerEntries, Entity::GetMessageHandlers() ); #endif //############################################################################# // Attribute Support // const Mover::IndexEntry Mover::AttributePointers[]= { ATTRIBUTE_ENTRY(Mover, LocalVelocity, localVelocity), ATTRIBUTE_ENTRY(Mover, LocalAcceleration, localAcceleration), ATTRIBUTE_ENTRY(Mover, WorldLinearVelocity, worldLinearVelocity), ATTRIBUTE_ENTRY(Mover, WorldLinearAcceleration, worldLinearAcceleration), ATTRIBUTE_ENTRY(Mover, MoverMass, moverMass), ATTRIBUTE_ENTRY(Mover, MomentOfInertia, momentOfInertia), ATTRIBUTE_ENTRY( Mover, PositiveLinearDragCoefficients, positiveLinearDragCoefficients ), ATTRIBUTE_ENTRY( Mover, NegativeLinearDragCoefficients, negativeLinearDragCoefficients ), ATTRIBUTE_ENTRY(Mover, AngularDragCoefficients, angularDragCoefficients), ATTRIBUTE_ENTRY(Mover, FrictionCoefficient, frictionCoefficient), ATTRIBUTE_ENTRY(Mover, ElasticityCoefficient, elasticityCoefficient), ATTRIBUTE_ENTRY(Mover, MinimumBounceSpeed, minimumBounceSpeed) }; Mover::AttributeIndexSet& Mover::GetAttributeIndex() { static Mover::AttributeIndexSet attributeIndex(ELEMENTS(Mover::AttributePointers), Mover::AttributePointers, Entity::GetAttributeIndex() ); return attributeIndex; } //############################################################################# // Model Support // //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::UpdateWorldMotion() { Check(this); // //--------------------------------------------------- // Move the accelerations back into world coordinates //--------------------------------------------------- // worldLinearAcceleration.Multiply( localAcceleration.linearMotion, localToWorld ); worldLinearVelocity.Multiply( localVelocity.linearMotion, localToWorld ); Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::UpdateLocalMotion() { Check(this); localVelocity.linearMotion.MultiplyByInverse( worldLinearVelocity, localToWorld ); localAcceleration.linearMotion.MultiplyByInverse( worldLinearAcceleration, localToWorld ); Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::ApplyWorldAccelerations(Scalar time_slice) { Check(this); Verify(time_slice > 0.0f); // //-------------------------------------------------- // Calculate the new position as p += v*t + a*.5*t*t //-------------------------------------------------- // Scalar half_t_squared = 0.5f * time_slice * time_slice; Vector3D position_delta; position_delta.Multiply(worldLinearAcceleration, half_t_squared); Check_Fpu(); position_delta.AddScaled( position_delta, worldLinearVelocity, time_slice ); Check_Fpu(); localOrigin.linearPosition.Add(localOrigin.linearPosition, position_delta); Check_Fpu(); position_delta.Multiply(localAcceleration.angularMotion, half_t_squared); Check_Fpu(); position_delta.AddScaled( position_delta, localVelocity.angularMotion, time_slice ); Check_Fpu(); Quaternion old_position = localOrigin.angularPosition; localOrigin.angularPosition.Add(old_position, position_delta); Check_Fpu(); // //----------------------------------- // Calculate our velocity as v += a*t //----------------------------------- // worldLinearVelocity.AddScaled( worldLinearVelocity, worldLinearAcceleration, time_slice ); Check_Fpu(); localVelocity.angularMotion.AddScaled( localVelocity.angularMotion, localAcceleration.angularMotion, time_slice ); Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::CalculateDrag( Vector3D *drag, const Vector3D &velocity, const Vector3D &positive_CODs, const Vector3D &negative_CODs, Scalar power ) { Environment *air = GetEnvironment(); Check(air); Vector3D temp,temp2; temp.MultiplyByInverse(air->GetWindVelocity(), localToWorld); temp += velocity; if (temp.x < 0.0f) { drag->x = negative_CODs.x; temp2.x = Power(-temp.x, power); Check_Fpu(); } else { drag->x = -positive_CODs.x; temp2.x = Power(temp.x, power); Check_Fpu(); } if (temp.y < 0.0f) { drag->y = negative_CODs.y; temp2.y = Power(-temp.y, power); Check_Fpu(); } else { drag->y = -positive_CODs.y; temp2.y = Power(temp.y, power); Check_Fpu(); } if (temp.z < 0.0f) { drag->z = negative_CODs.z; temp2.z = Power(-temp.z, power); Check_Fpu(); } else { drag->z = -positive_CODs.z; temp2.z = Power(temp.z, power); Check_Fpu(); } *drag *= air->airDensity; drag->Multiply(*drag, temp2); Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::ApplyAirResistanceAndGravity(Scalar power) { Check(this); // //------------------------------------------------------------------------- // Apply drag to the system, allowing for different drag numbers based upon // the direction of motion along the axis //------------------------------------------------------------------------- // Vector3D acceleration; CalculateDrag( &acceleration, localVelocity.linearMotion, positiveLinearDragCoefficients, negativeLinearDragCoefficients, power ); localAcceleration.linearMotion += acceleration; acceleration.Multiply(angularDragCoefficients, localVelocity.angularMotion); localAcceleration.angularMotion -= acceleration; // //--------------------------- // Apply gravity to the craft //--------------------------- // UpdateWorldMotion(); worldLinearAcceleration.y -= GetEnvironment()->gravityConstant; Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::ApplyLocalForce( const Vector3D &force, const Vector3D &moment ) { Check(this); Check(&force); Check(&moment); Vector3D acceleration; Verify(!Small_Enough(moverMass)); acceleration.Divide(force, moverMass); ApplyLocalAcceleration(acceleration, moment); Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::ApplyLocalAcceleration( const Vector3D &acceleration, const Vector3D &moment ) { Check(this); Check(&acceleration); Check(&moment); localAcceleration.linearMotion += acceleration; Vector3D torque; torque.Cross(moment, acceleration); torque *= momentOfInertia; localAcceleration.angularMotion += torque; Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Logical Mover::NoDeadReckoner() { Check(this); // //------------------------------------------------------------------------- // If we are the replicant instance and we are not yet past the anticipated // time for the next event, project out to the next event //------------------------------------------------------------------------- // projectedOrigin = updateOrigin; Check_Fpu(); return False; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Logical Mover::LinearDeadReckoner() { Check(this); Logical lerp_mode; Scalar time_slice; // //------------------------------------------------------------------------- // If we are the replicant instance and we are not yet past the anticipated // time for the next event, project out to the next event //------------------------------------------------------------------------- // if (GetInstance() == ReplicantInstance && lastPerformance < nextUpdate) { time_slice = nextUpdate - lastUpdate; lerp_mode = True; } else { time_slice = lastPerformance - lastUpdate; lerp_mode = False; } // //--------------------------------------- // Calculate the new position as p += v*t //--------------------------------------- // Vector3D position_delta; position_delta.Multiply(updateVelocity.linearMotion, time_slice); projectedOrigin.linearPosition.Add( updateOrigin.linearPosition, position_delta ); // //------------------------------- // Handle projecting the rotation //------------------------------- // position_delta.Multiply(updateVelocity.angularMotion, time_slice); projectedOrigin.angularPosition.Add( updateOrigin.angularPosition, position_delta ); projectedVelocity = updateVelocity; Check_Fpu(); return lerp_mode; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Logical Mover::AcceleratedDeadReckoner() { Check(this); Logical lerp_mode; Scalar time_slice; // //------------------------------------------------------------------------- // If we are the replicant instance and we are not yet past the anticipated // time for the next event, project out to the next event //------------------------------------------------------------------------- // if (GetInstance() == ReplicantInstance && lastPerformance < nextUpdate) { time_slice = nextUpdate - lastUpdate; lerp_mode = True; } else { time_slice = lastPerformance - lastUpdate; lerp_mode = False; } // //-------------------------------------------------- // Calculate the new position as p += v*t + a*.5*t*t //-------------------------------------------------- // Scalar half_t_squared = 0.5f * time_slice * time_slice; Vector3D position_delta; position_delta.Multiply(updateAcceleration.linearMotion, half_t_squared); position_delta.AddScaled( position_delta, updateVelocity.linearMotion, time_slice ); projectedOrigin.linearPosition.Add( updateOrigin.linearPosition, position_delta ); // //------------------------------- // Handle projecting the rotation //------------------------------- // position_delta.Multiply(updateAcceleration.angularMotion, half_t_squared); position_delta.AddScaled( position_delta, updateVelocity.angularMotion, time_slice ); projectedOrigin.angularPosition.Add( updateOrigin.angularPosition, position_delta ); // //----------------------------------- // Calculate our velocity as v += a*t //----------------------------------- // if (GetInstance() == ReplicantInstance) { projectedVelocity.linearMotion.AddScaled( updateVelocity.linearMotion, worldLinearAcceleration, time_slice ); projectedVelocity.angularMotion.AddScaled( updateVelocity.angularMotion, localAcceleration.angularMotion, time_slice ); } Check_Fpu(); return lerp_mode; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::DeadReckon(Scalar time_slice) { Check(this); // //------------------------------ // Run the chosen dead reckoning //------------------------------ // Verify(GetInstance() == ReplicantInstance); if (deadReckoner) { // //--------------------------------------------------------------------- // Merge the projected origin with the current origin if we are in lerp // mode. If not, just copy the projected origin into the local origin //--------------------------------------------------------------------- // if ((this->*deadReckoner)()) { Scalar percent = time_slice / ((nextUpdate - lastPerformance) + time_slice); // for the RP412CAMLOG trace at the end of this function gLastLerpUsed = True; gLastPercent = percent; // //------------------------------------------ // Do a spherical lerp on the angular motion //------------------------------------------ // localOrigin.angularPosition.Lerp( localOrigin.angularPosition, projectedOrigin.angularPosition, percent ); localVelocity.angularMotion.Lerp( localVelocity.angularMotion, projectedVelocity.angularMotion, percent ); // //------------------------- // Spline the linear motion //------------------------- // #if 0 CubicCurve spline( localOrigin.linearPosition, worldLinearVelocity, projectedOrigin.linearPosition, projectedVelocity.linearMotion ); spline.Evaluate( percent, &localOrigin.linearPosition, &worldLinearVelocity ); #else localOrigin.linearPosition.Lerp( localOrigin.linearPosition, projectedOrigin.linearPosition, percent ); worldLinearVelocity.Lerp( worldLinearVelocity, projectedVelocity.linearMotion, percent ); #endif } else { gLastLerpUsed = False; // snapped, not blended localOrigin = projectedOrigin; worldLinearVelocity = projectedVelocity.linearMotion; localVelocity.angularMotion = projectedVelocity.angularMotion; } // //---------------------------------------------------- // Update the collision volume and the local variables //---------------------------------------------------- // if (IsCollisionVolume()) { MoveCollisionVolume(); } else { localToWorld = localOrigin; } UpdateLocalMotion(); // // RP412CAMLOG: is a replicant's motion actually uniform? // // Measured HERE, in the replicant's own step, and nowhere else. // Every previous attempt at this question sampled from another // clock - the camera's step grid, or an arriving packet's // timestamp - and two independent clocks alias against each other // whatever the game is doing, so those numbers could never // separate a real hitch from the measurement's own beat. This one // has a single frame of reference: consecutive steps of the entity // being asked about. // // percent is the whole mechanism above: it is how far this step // moves toward the projected position, and it depends on // nextUpdate being a decent guess at when the next packet lands. // If that guess is poor the fraction swings, and swinging fraction // is uneven motion no matter how clean the packets were. // // One entity only - the first replicant seen - because these // counters are shared and a grid full of pods would blend into // noise. // if (RPCameraLog()) { if (!gTracedLatched) { gTracedLatched = True; gTracedEntity = GetEntityID(); } if (gTracedEntity == GetEntityID()) { static Scalar next_say = 0.0f; static Point3D last_pos(0.0f, 0.0f, 0.0f); static Logical have_last = False; static int steps = 0; static int spikes = 0; static int stalls = 0; static int lerped = 0; static Scalar mean_step = 0.0f; static Scalar min_percent = 1.0f; static Scalar max_percent = 0.0f; static Scalar worst_error = 0.0f; static Scalar last_distance = 0.0f; static Scalar recent[16]; static Scalar frozen[16]; static int recent_next = 0; static int recent_count = 0; static Logical captured = False; static Scalar captured_ratio = 0.0f; static Scalar captured_percent = 0.0f; static int seq_stalls = 0; static int seq_spikes = 0; ++steps; if (have_last) { Vector3D moved; moved.Subtract(localOrigin.linearPosition, last_pos); Scalar distance = moved.Length(); // // The same test the renderer applies to drawn frames: // this step against the one before it, not against a // running mean. // // A running mean is blind to an alternating pattern - // high, low, high, low averages to the mean and nothing // ever looks anomalous - which is why this trace has // been reporting zero spikes and zero stalls while the // renderer, comparing consecutive frames, counted // fifteen to forty-six stalls in the same motion. The // mean test only ever ruled out DRIFT. // if (distance < 50.0f) { // // Keep the last sixteen steps rolling, and freeze a // copy the moment a stall is seen. // // The first version of this printed the first twelve // steps of each window and they came back immaculate // - 1.029, 1.031, 1.032, monotonic to a tenth of a // percent - while the same window counted sixteen // stalls among the other two hundred and thirty // nine. Sampling a calm quarter second says nothing // about a tick that happens elsewhere. The sample // has to be triggered BY the event. // recent[recent_next] = distance; recent_next = (recent_next + 1) % 16; if (recent_count < 16) { recent_count++; } if (last_distance > 0.001f) { Scalar sequential = distance / last_distance; if (sequential < 0.4f) { ++seq_stalls; if (!captured && recent_count == 16) { captured = True; captured_ratio = sequential; captured_percent = gLastPercent; for (int c = 0; c < 16; c++) { frozen[c] = recent[(recent_next + c) % 16]; } } } else if (sequential > 2.5f) { ++seq_spikes; } } last_distance = distance; } if (distance > 50.0f) { mean_step = 0.0f; // respawn, not motion } else if (mean_step > 0.01f) { Scalar ratio = distance / mean_step; if (ratio > 2.5f) { ++spikes; } else if (ratio < 0.4f) { ++stalls; } mean_step = mean_step * 0.9f + distance * 0.1f; } else { mean_step = distance; } } last_pos = localOrigin.linearPosition; have_last = True; if ((Scalar) Now() >= next_say) { if (next_say > 0.0f) { DEBUG_STREAM << "CamLog: replicant motion - " << steps << " own steps, " << spikes << " spike(s), " << stalls << " stall(s), " << lerped << " lerped, percent " << min_percent << ".." << max_percent << ", mean step " << mean_step << "m, predicting " << predictedInterval << "s worst miss " << worst_error << "s\n" << std::flush; DEBUG_STREAM << "CamLog: replicant sequence - " << seq_stalls << " stall(s), " << seq_spikes << " spike(s) against the PREVIOUS step"; if (captured) { // // The fifteen steps leading into a stall and the // stall itself, last in the list. // DEBUG_STREAM << "; at a stall (ratio " << captured_ratio << ", percent " << captured_percent << "):"; for (int s = 0; s < 16; s++) { DEBUG_STREAM << " " << frozen[s]; } } else { DEBUG_STREAM << "; no stall caught this window"; } DEBUG_STREAM << "\n" << std::flush; DEBUG_STREAM << "CamLog: replicant arrivals - widest gap " << widestGap << "s, " << longGapCount << " long, " << queuedGapCount << " queued (" << ((longGapCount > 0 && queuedGapCount > 0) ? "our loop stalled" : (longGapCount > 0 ? "sender went quiet" : "clean")) << ")\n" << std::flush; } widestGap = 0.0f; longGapCount = 0; queuedGapCount = 0; next_say = ((Scalar) Now()) + 5.0f; steps = 0; spikes = 0; stalls = 0; lerped = 0; min_percent = 1.0f; max_percent = 0.0f; worst_error = 0.0f; captured = False; seq_stalls = 0; seq_spikes = 0; } { Scalar missed = (predictionError < 0.0f) ? -predictionError : predictionError; if (missed > worst_error) { worst_error = missed; } } if (gLastLerpUsed) { ++lerped; if (gLastPercent < min_percent) { min_percent = gLastPercent; } if (gLastPercent > max_percent) { max_percent = gLastPercent; } } } } } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::PerformAndWatch( const Time &till, MemoryStream *update_stream ) { Check(this); Check(&till); int i; // //-------------------------------------------------------------------------- // Make sure that the time into the simulations is stable. If a half-second // delay occurs, or we are in stasis, just bring everything up to date //-------------------------------------------------------------------------- // Scalar time_slice = till - lastPerformance; if (time_slice < SMALL) { Tell("No time!\n"); Bye_Bye: WriteSimulationUpdate(update_stream); return; } if (GetSimulationState() == StasisState || time_slice > 0.5f) { lastPerformance = till; if (GetSimulationState() == StasisState) { lastUpdate = till; } if (subsystemArray) { Check_Pointer(subsystemArray); for (i=0; iSetLastPerformance(till); } } } //SetSimulationState(DefaultState); goto Bye_Bye; } // //------------------------------------ // Set up for local motion calculation //------------------------------------ // localVelocity.linearMotion.MultiplyByInverse( worldLinearVelocity, localToWorld ); localAcceleration = Motion::Identity; previousOrigin = localOrigin; // //----------------------- // Process the subsystems //----------------------- // Entity::PerformAndWatch(till, update_stream); // //----------------------------------------------- // Make sure the position quaternion stays stable // // Frame-counting, so it only runs on the frame-coupled path - fixed // steps do the same thing in BeginStep, counted in STEPS, because // "every 20 frames" lands at a different point of the step sequence // on every machine and rounding at different points is drift. //----------------------------------------------- // if (Simulation::FixedStep() <= (Scalar) 0 && ++normalizeCount >= 20) { localOrigin.angularPosition.Normalize(); normalizeCount = 0; } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // // The per-STEP set-up. This is the same work Mover::PerformAndWatch does // once per frame above - and once per frame is exactly wrong under fixed // stepping: the thrusters ADD their forces into localAcceleration every // step, so an accumulator cleared per frame carries step one's thrust // into step two whenever a frame holds two steps. How many steps a frame // holds depends on wall-clock jitter, which made identical runs diverge // by a quarter of a metre while sitting still on the pad. // // Idempotent on purpose: the frame-level copy still runs first on every // path, and repeating this at each step start is a recompute from // current state, not an accumulation. // void Mover::BeginStep() { Check(this); localVelocity.linearMotion.MultiplyByInverse( worldLinearVelocity, localToWorld ); localAcceleration = Motion::Identity; previousOrigin = localOrigin; if (++normalizeCount >= 20) { localOrigin.angularPosition.Normalize(); normalizeCount = 0; } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::ResetUpdateIntervals() { Check(this); updateIntervalCount = 0; updateIntervalWrite = 0; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // // Estimate how long until the next update for this entity arrives. // // This is not a cosmetic guess. DeadReckon blends toward the projected // origin by // // percent = time_slice / ((nextUpdate - lastPerformance) + time_slice) // // so the prediction sets how far every single step moves. The original code // predicted the next gap from the one previous gap. On a LAN that was fine, // because the gaps were all alike. Over the internet a late packet doubles // the prediction, percent collapses toward zero, the entity barely advances // for a step and then catches up on the following ones - which is a visible // tick. Measured on a live Steam connection at about 1.4 a second, with // percent bottoming out at 0.014 against a normal range of 0.27 to 0.95. // // A median has a breakdown point of half its samples, so one straggler - or // three - moves it not at all, while a real change in the send rate still // carries it within a few updates. That is the whole trick: ignore the // outlier, follow the trend. // Scalar Mover::PredictUpdateInterval(Scalar latest) { Check(this); // // Only plausible gaps go into the window. Letting a join or a stall in // would poison the estimate for the next eight updates - precisely when // the entity is most conspicuous, just after it appears. // if (latest > kMinimumUpdateInterval && latest < kOutlierUpdateInterval) { updateIntervals[updateIntervalWrite] = latest; updateIntervalWrite = (updateIntervalWrite + 1) % UpdateIntervalSamples; if (updateIntervalCount < UpdateIntervalSamples) { updateIntervalCount++; } } // // Too few samples to hold an opinion. Fall back to the gap we just saw // rather than inventing a rate we have no evidence for - but clamp it // like any other answer. Leaving this path unclamped let a 2.05s gap // through in the first updates after an entity appeared, which drove // the blend fraction to 0.0097 and stalled the step. That is every // respawn, and it is exactly when the pod is being watched. // if (updateIntervalCount < 3) { return ClampPredictedInterval(latest); } // // Insertion sort - the window is eight samples, and this runs once per // arriving packet per entity. // Scalar sorted[UpdateIntervalSamples]; int i; for (i = 0; i < updateIntervalCount; i++) { sorted[i] = updateIntervals[i]; } for (i = 1; i < updateIntervalCount; i++) { Scalar value = sorted[i]; int j = i - 1; while (j >= 0 && sorted[j] > value) { sorted[j + 1] = sorted[j]; j--; } sorted[j + 1] = value; } return ClampPredictedInterval(sorted[updateIntervalCount / 2]); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::ReadUpdateRecord(Simulation::UpdateRecord *record) { Check(this); Check_Pointer(record); switch (record->recordID) { case DefaultUpdateModelBit: { // //--------------------------------------- // Precalculation for next update time //--------------------------------------- // nextUpdate = Now(); Scalar diff = nextUpdate - lastUpdate; Scalar anchorInterval = (Scalar) 0; if (diff < 10.0f) { if (UseMedianPrediction()) { Scalar predicted = PredictUpdateInterval(diff); // // Anchor the projection to the SENDER's timeline, below, // once Entity::ReadUpdateRecord has moved lastUpdate to // the sampling moment RP412NETCLOCK worked out. // anchorInterval = predicted; // // Score the previous prediction against the gap that // actually just elapsed - a true one-step-ahead error, // kept per entity so a trace reads the entity it is // watching and not whichever one updated last. // if (predictedInterval > 0.0f) { predictionError = predictedInterval - diff; } predictedInterval = predicted; // // Arrival statistics, for telling a quiet sender from // our own stalled loop. See the members. // if (diff > widestGap) { widestGap = diff; } if (diff > kLongGapThreshold) { longGapCount++; } if (diff < kQueuedGapThreshold) { queuedGapCount++; } } else { nextUpdate.ticks += nextUpdate.ticks - lastUpdate.ticks; } } else { // // The stream was interrupted - a join, a pause, a long // stall. Nothing recorded before it describes the rate // now, so start the window over. // ResetUpdateIntervals(); } // // RP412CAMLOG: is constant-velocity extrapolation actually // accurate over one interval, or is the pod manoeuvring? // // The corrections measured 0.25 to 0.66m against a step of // about a metre, which is what collapses one step to a third // and shows as the tick. At 52 m/s half a metre is ten // milliseconds of travel, so the question is whether we are // evaluating the projection at the wrong INSTANT or whether the // pod simply is not going in a straight line. // // This settles it without involving any clock we do not trust: // take the position and velocity the sender reported last time, // carry them forward by the difference between the two SENDER // timestamps, and compare against the position the sender // reports now. Both stamps come from the same machine, so // latency, clock offset and RP412NETCLOCK play no part - it // measures the prediction and nothing else. // // Split the error along the direction of travel and across it. // Error ALONG the path is time: divided by speed it IS the // number of milliseconds the window is out by, and its sign // says which way. Error ACROSS the path cannot be a timing // problem at all - that is a pod turning, and no clock fix // would touch it. // if (RPCameraLog()) { UpdateRecord *sample = (UpdateRecord*)record; if (haveSenderSample) { Scalar dt = sample->timeStamp - senderStamp; Scalar speed = senderVelocity.Length(); if (dt > 0.001f && dt < 1.0f && speed > 1.0f) { Vector3D error; error.x = sample->localOrigin.linearPosition.x - (senderPosition.x + senderVelocity.x * dt); error.y = sample->localOrigin.linearPosition.y - (senderPosition.y + senderVelocity.y * dt); error.z = sample->localOrigin.linearPosition.z - (senderPosition.z + senderVelocity.z * dt); Scalar along = (error.x * senderVelocity.x + error.y * senderVelocity.y + error.z * senderVelocity.z) / speed; Vector3D across; across.x = error.x - (senderVelocity.x / speed) * along; across.y = error.y - (senderVelocity.y / speed) * along; across.z = error.z - (senderVelocity.z / speed) * along; gPredictSamples++; gPredictAlong += along; gPredictAlongAbs += (along < 0.0f) ? -along : along; gPredictAcross += across.Length(); gPredictMilliseconds += (along / speed) * 1000.0f; Scalar now_say = (Scalar) Now(); if (now_say >= gPredictNextSay) { if (gPredictNextSay > 0.0f && gPredictSamples > 0) { Scalar mean_along = gPredictAlong / gPredictSamples; Scalar mean_across = gPredictAcross / gPredictSamples; Scalar mean_ms = gPredictMilliseconds / gPredictSamples; DEBUG_STREAM << "CamLog: prediction error - " << gPredictSamples << " intervals, along " << mean_along << "m (" << mean_ms << "ms of travel), across " << mean_across << "m, verdict " << (((mean_along < 0.0f ? -mean_along : mean_along) > mean_across * 2.0f) ? "TIMING - the window is off" : ((mean_across > (mean_along < 0.0f ? -mean_along : mean_along) * 2.0f) ? "MANOEUVRE - the pod is turning" : "mixed")) << "\n" << std::flush; } gPredictNextSay = now_say + 5.0f; gPredictSamples = 0; gPredictAlong = 0.0f; gPredictAlongAbs = 0.0f; gPredictAcross = 0.0f; gPredictMilliseconds = 0.0f; } } } senderStamp = sample->timeStamp; senderPosition = sample->localOrigin.linearPosition; senderVelocity = sample->worldLinearVelocity; haveSenderSample = True; } // //--------------------------------------- // Handle updating the entity information //--------------------------------------- // Entity::ReadUpdateRecord(record); // // Put the projection deadline on the SENDER's timeline. // // The dead reckoner projects to updateOrigin + velocity * // (nextUpdate - lastUpdate), so that difference is a DISTANCE // once multiplied by speed - and a pod at 52 m/s turns every // millisecond in it into 52mm of target. // // lastUpdate is the sampling moment RP412NETCLOCK computed, on // the sender's clock. Setting nextUpdate from Now() measured the // gap between two different timelines, so it came out as the // interval PLUS however late this particular packet happened to // be. Fifteen milliseconds of ordinary jitter became three // quarters of a metre of target error, which is enough to // collapse a one metre step to a third - and only on the packets // that ran late, which is exactly the intermittent tick that was // reported. // // Anchored to lastUpdate the difference is the predicted // interval exactly, so the target depends on what the sender // said and how fast it is going, and not at all on the route the // packet took to reach us. // if (anchorInterval > (Scalar) 0) { nextUpdate = lastUpdate; nextUpdate += anchorInterval; } // //----------------------- // Update the motion data //----------------------- // UpdateRecord *update = (UpdateRecord*)record; localAcceleration = update->localAcceleration; worldLinearAcceleration = update->worldLinearAcceleration; updateVelocity.linearMotion = update->worldLinearVelocity; updateVelocity.angularMotion = update->localVelocity.angularMotion; updateAcceleration.linearMotion = update->worldLinearAcceleration; updateAcceleration.angularMotion = update->localAcceleration.angularMotion; // //----------------------------------------- // Update the collision volume if necessary //----------------------------------------- // if (IsCollisionVolume()) { MoveCollisionVolume(); } } break; default: Entity::ReadUpdateRecord(record); break; } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::WriteUpdateRecord( Simulation::UpdateRecord *record, int update_model ) { Check(this); Check_Pointer(record); switch (update_model) { case DefaultUpdateModelBit: { Entity::WriteUpdateRecord(record, update_model); UpdateRecord *update = (UpdateRecord*)record; update->recordLength = sizeof(*update); update->localVelocity = localVelocity; update->localAcceleration = localAcceleration; update->worldLinearVelocity = worldLinearVelocity; update->worldLinearAcceleration = worldLinearAcceleration; updateVelocity.linearMotion = worldLinearVelocity; updateVelocity.angularMotion = localVelocity.angularMotion; updateAcceleration.linearMotion = worldLinearAcceleration; updateAcceleration.angularMotion = localAcceleration.angularMotion; } break; default: Entity::WriteUpdateRecord(record, update_model); break; } Check_Fpu(); } //############################################################################# // Collision support // //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::MoveCollisionVolume() { // //--------------------------------------------------- // Make sure that there is a collision volume to move //--------------------------------------------------- // Check(this); if (!collisionVolumeCount) { Check_Fpu(); return; } // //------------------------------------------------------------------------ // Set up the extents of the collision volume from the template and the // current position. We must find the center point of the template volume // and rotate it about the y axis //------------------------------------------------------------------------ // Check(collisionTemplate); Check(collisionVolume); Verify(collisionVolumeCount == 1); Verify(collisionTemplate->solidType == BoxedSolid::YAxisCylinderType); localToWorld = localOrigin; Point3D centerPoint; centerPoint.x = (collisionTemplate->minX + collisionTemplate->maxX) * 0.5f; centerPoint.y = (collisionTemplate->minY + collisionTemplate->maxY) * 0.5f; centerPoint.z = (collisionTemplate->minZ + collisionTemplate->maxZ) * 0.5f; Vector3D radius; radius.x = collisionTemplate->maxX - centerPoint.x; radius.y = collisionTemplate->maxY - centerPoint.y; radius.z = collisionTemplate->maxZ - centerPoint.z; Point3D rotated; rotated.Multiply(centerPoint, localToWorld); collisionVolume->minX = rotated.x - radius.x; collisionVolume->maxX = rotated.x + radius.x; collisionVolume->minY = rotated.y - radius.y; collisionVolume->maxY = rotated.y + radius.y; collisionVolume->minZ = rotated.z - radius.z; collisionVolume->maxZ = rotated.z + radius.z; // //------------------------------------------------------------ // Now, Find the smallest node containing our collision column //------------------------------------------------------------ // if (GetInstance() != ReplicantInstance) { InterestManager *interest_mgr = application->GetInterestManager(); Check(interest_mgr); InterestZone *zone = interest_mgr->GetInterestZone(interestZoneID); Check(zone); BoxedSolidTree* tree = zone->GetCollisionRoot(); Check(tree); containedByNode = tree->FindSmallestNodeContainingColumn(*collisionVolume); } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // BoxedSolidCollisionList* Mover::AllocateCollisionList() { Check(this); // //----------------------------------------------------------- // Find the correct collision list to use, and reset to empty //----------------------------------------------------------- // BoxedSolidCollisionList *collision_list; if (lastCollisionList == collisionLists) { collision_list = &collisionLists[1]; } else { collision_list = collisionLists; } Check(collision_list); collision_list->Reset(); return collision_list; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // BoxedSolidCollisionList* Mover::GetCurrentCollisions(BoxedSolidCollisionList *collision_list) { Check(this); if (!collision_list) { collision_list = AllocateCollisionList(); } Check(collision_list); // //--------------------------------- // Test against the tangible movers //--------------------------------- // Check(collisionAssistant); CollisionAssistant::MovingEntityIterator iterator(collisionAssistant); Entity *entity; Check(collisionVolume); while ((entity = iterator.ReadAndNext()) != NULL) { // //------------------------------------------------------------------ // If we are checking against ourselves, or something more than 50m // away, skip it //------------------------------------------------------------------ // Check(entity); if (entity == this) { continue; } Vector3D delta; delta.Subtract( entity->localOrigin.linearPosition, localOrigin.linearPosition ); if (delta.LengthSquared() > 2500.0f) { continue; } // //-------------------------------------------------- // If we have a mover class object, check against it //-------------------------------------------------- // if (entity->IsDerivedFrom(*Mover::GetClassDerivations())) { Mover *mover = (Mover*)entity; Check(mover); Check(mover->collisionVolume); CheckAgainstBoxedSolidChain(collision_list, mover->collisionVolume); } // //----------------------------------------------------------------------- // If we have a door, check against its subsystems if we are close enough // for it to matter //----------------------------------------------------------------------- // else if (entity->IsDerivedFrom(*DoorFrame::GetClassDerivations())) { DoorFrame *door_frame = (DoorFrame*)entity; Check(door_frame); for (int i=0; iGetSubsystemCount(); ++i) { Door *door = (Door*)door_frame->GetSubsystem(i); CheckAgainstBoxedSolidChain( collision_list, door->GetFirstBoxedSolid() ); } } } // //------------------------------ // Test against the static world //------------------------------ // containedByNode->FindBoundingBoxesContaining( collisionVolume, *collisionVolume, *collision_list ); Check_Fpu(); return collision_list; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // BoxedSolid* Mover::FindBoxedSolidHitBy( Line *line, Entity *except ) { Check(this); Check(line); // //------------------------------------------------------------------------- // Calculate the midpoint of the line, and sweep a sphere out around the // line from that point, including an extra 50 meters. This extra distance // takes into account the doors... //------------------------------------------------------------------------- // Point3D center; Scalar radius = line->length * 0.5f; line->Project(radius, ¢er); // //--------------------------------- // Test against the tangible movers //--------------------------------- // Check(collisionAssistant); CollisionAssistant::MovingEntityIterator iterator(collisionAssistant); Entity *entity; BoxedSolid *solid = NULL, *result; while ((entity = iterator.ReadAndNext()) != NULL) { // //--------------------------------------------------------------- // If we are checking against ourselves or the exception, skip it //--------------------------------------------------------------- // Check(entity); if (entity == this || except && except == entity) { continue; } // //------------------------------------------------------------------- // If we have a mover class object, check against it. If we have no // collision volume, we are just using a line, so just run it against // the collision volume chain //------------------------------------------------------------------- // if (entity->IsDerivedFrom(*Mover::GetClassDerivations())) { Mover *mover = (Mover*)entity; Check(mover); Check(mover->collisionVolume); // //----------------------------------------------------------------- // If the mover is close enough to the radius of the line, check it //----------------------------------------------------------------- // Vector3D delta; delta.Subtract(entity->localOrigin.linearPosition, center); Scalar r2 = mover->collisionVolume->maxX - mover->collisionVolume->minX; r2 += mover->collisionVolume->maxY - mover->collisionVolume->minY; r2 *= 0.5f; r2 += radius; if (delta.LengthSquared() > r2*r2) { continue; } result = CheckLineAgainstBoxedSolidChain(line, mover->collisionVolume); if (result) { Check(result); solid = result; } } // //----------------------------------------------------------------------- // If we have a door, check against its subsystems if we are close enough // for it to matter //----------------------------------------------------------------------- // else if (entity->IsDerivedFrom(*DoorFrame::GetClassDerivations())) { // //----------------------------------------------------------------- // If the mover is close enough to the radius of the line, check it //----------------------------------------------------------------- // Vector3D delta; delta.Subtract(entity->localOrigin.linearPosition, center); Scalar r2 = radius + 50.0f; if (delta.LengthSquared() > r2*r2) { continue; } DoorFrame *door_frame = (DoorFrame*)entity; Check(door_frame); for (int i=0; iGetSubsystemCount(); ++i) { Door *door = (Door*)door_frame->GetSubsystem(i); result = CheckLineAgainstBoxedSolidChain( line, door->GetFirstBoxedSolid() ); if (result) { Check(result); solid = result; } } } } // //------------------------------ // Test against the static world //------------------------------ // InterestManager *interest_mgr = application->GetInterestManager(); Check(interest_mgr); InterestZone *zone = interest_mgr->GetInterestZone(interestZoneID); Check(zone); BoxedSolidTree* tree = zone->GetCollisionRoot(); Check(tree); result = (BoxedSolid*)tree->FindBoundingBoxHitBy(line); if (result) { Check(result); solid = result; } Check_Fpu(); return solid; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // BoxedSolidCollisionList* Mover::CollideCenterOfMotion( Line *line, BoxedSolidCollisionList *list ) { Check(this); Check(line); // //----------------------------------------------------------- // Find the correct collision list to use, and reset to empty //----------------------------------------------------------- // if (!list) { list = AllocateCollisionList(); } Check(list); // //------------------------------------------------------------------------ // If the length of the line has changed, we must reposition the collision // volume appropriately //------------------------------------------------------------------------ // BoxedSolid *solid = FindBoxedSolidHitBy(line, NULL); if (solid && IsCollisionVolume()) { line->FindEnd(&localOrigin.linearPosition); MoveCollisionVolume(); ExtentBox slice; slice.Intersect(*collisionVolume, *solid); Verify(list->GetCollisionsLeft()); list->AddCollisionToList(solid, slice); } Check_Fpu(); return list; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::ProcessCollisionList( BoxedSolidCollisionList *collisions, Scalar time_slice, const Point3D &old_position, Damage *damage ) { Check(this); Check(collisions); Verify(time_slice > 0.0f); Check(&old_position); Check_Pointer(damage); damage->damageAmount = 0.0f; damage->damageType = Damage::CollisionDamageType; damage->impactPoint = Point3D::Identity; if (collisions->GetCollisionCount()) { // //------------------------------------------------------------------ // Reduce the number of collisions we have to play with based on our // velocity //------------------------------------------------------------------ // collisions->ReduceCollisionList(worldLinearVelocity); // //----------------------------------------------------------------- // Setup up the totaling variables to handle averaging out multiple // collisions //----------------------------------------------------------------- // int total_collisions = 0; Vector3D resultant_velocity = Vector3D::Identity; Point3D resultant_position = Point3D::Identity; Vector3D resultant_normal = Vector3D::Identity; Vector3D initial_velocity = worldLinearVelocity; Vector3D initial_position = localOrigin.linearPosition; Scalar total_damage = 0.0f; // //--------------------------------------------------------------------- // For each hit in the list, process it, and if the collision is // determined to be valid, bounce it and add the result into the others //--------------------------------------------------------------------- // for (int i=0; iGetRealCollisions(); ++i) { // //---------------------------------------------------------- // Make sure to bounce the vehicle from the correct location //---------------------------------------------------------- // worldLinearVelocity = initial_velocity; localOrigin.linearPosition = initial_position; damage->damageAmount = 0.0f; ProcessCollision( time_slice, (*collisions)[i], old_position, damage ); if (damage->damageAmount > 0.0f) { ++total_collisions; resultant_velocity += worldLinearVelocity; resultant_position += localOrigin.linearPosition; resultant_normal += damage->surfaceNormal; total_damage += damage->damageAmount; ExtentBox *box = &(*collisions)[i].collisionSlice; damage->impactPoint.x += 0.5 * ( box->minX + box->maxX - (collisionVolume->minX - collisionVolume->maxX) ); damage->impactPoint.y += 0.5 * ( box->minY + box->maxY - (collisionVolume->minY - collisionVolume->maxY) ); damage->impactPoint.z += 0.5 * ( box->minZ + box->maxZ - (collisionVolume->minZ - collisionVolume->maxZ) ); } } // //----------------------------------------------------------------- // If we collided with more than one thing, average out the results //----------------------------------------------------------------- // if (total_collisions > 1) { worldLinearVelocity.Divide(resultant_velocity, total_collisions); localOrigin.linearPosition.Divide( resultant_position, total_collisions ); damage->surfaceNormal.Vector3D::Divide( resultant_normal, total_collisions ); goto Figure_Normal; } // //------------------------------------------------------ // Otherwise, just set up the positions from the results //------------------------------------------------------ // else if (total_collisions == 1) { worldLinearVelocity = resultant_velocity; localOrigin.linearPosition = resultant_position; damage->surfaceNormal.operator=(resultant_normal); // //--------------------------------------------------------- // Figure out the normal, and calculate the collision force //--------------------------------------------------------- // Figure_Normal: if (Small_Enough(damage->surfaceNormal.LengthSquared())) { damage->surfaceNormal.x = 0.0f; damage->surfaceNormal.y = 1.0f; damage->surfaceNormal.z = 0.0f; } else { damage->surfaceNormal.Normalize(damage->surfaceNormal); } MoveCollisionVolume(); damage->damageAmount = total_damage; damage->damageForce.Subtract(worldLinearVelocity, initial_velocity); } lastCollisionList = collisions; } else { lastCollisionList = NULL; } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::ProcessCollision( Scalar time_slice, BoxedSolidCollision &collision, const Point3D &old_position, Damage *damage ) { Check(this); Verify(time_slice > 0.0f); Check(&collision); Check(&old_position); Check_Pointer(damage); Scalar penetration; // //------------------------------------------------------------------------ // If we really have a collision, do a static bounce off of the normal // generated. This is default behavior, and any derived class should make // sure to handle any handshaking that needs to be done //------------------------------------------------------------------------ // if ( collisionVolume->ProcessCollision( collision, worldLinearVelocity, lastCollisionList, &damage->surfaceNormal, &penetration ) ) { Max_Clamp(penetration, time_slice); Scalar r = penetration / time_slice; Scalar elasticity = elasticityCoefficient; Scalar friction = frictionCoefficient; damage->damageAmount = StaticBounce( old_position, time_slice, r, damage->surfaceNormal, &elasticity, minimumBounceSpeed, &friction ); } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::StartCollisionAssistant() { Check(this); Verify(collisionAssistant == NULL); collisionAssistant = CollisionAssistant::Make(this); Register_Object(collisionAssistant); Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Scalar Mover::StaticBounce( const Point3D &, //old_position, Scalar delta_t, Scalar penetration, const Normal &normal, Scalar *elasticity, Scalar bounce_min, Scalar *friction ) { Check(this); Check(&normal); Check_Pointer(elasticity); Check_Pointer(friction); Verify(penetration >= 0.0f && penetration <= 1.0f); Verify(*elasticity >= 0.0f && *elasticity <= 1.0f); Verify(*friction >= 0.0f); Verify(delta_t > SMALL); // penetration = 1.0f; // HACK - should keep stuff from going through the floor // //----------------------------------------------------------------------- // Calculate the impact speed and vectors. If we didn't hit fast enough, // don't do any bounce //----------------------------------------------------------------------- // Scalar impact = worldLinearVelocity * normal; Vector3D vn,vp; vn.Multiply(normal, impact); vp.Subtract(worldLinearVelocity, vn); if (impact > 0.0f) { Check_Fpu(); return 0.0f; } if (-impact <= bounce_min * delta_t) { *elasticity = 0.0f; } // //-------------------------------------- // Calculate the energy lost to friction //-------------------------------------- // Scalar resistance = vp.Length(); if (Small_Enough(resistance)) { *friction = resistance = 0.0f; } else { resistance = 1.0f + *friction * (1.0f + *elasticity) * impact / resistance; if (resistance < 0.0f) { *friction = resistance = 0.0f; } } // //---------------------------------------------------- // Compute the velocity delta created by the collision //---------------------------------------------------- // Scalar temp = resistance - 1.0f; Vector3D delta_v; delta_v.Multiply(worldLinearVelocity, temp); temp = resistance + *elasticity; delta_v.AddScaled(delta_v, vn, -temp); // //------------------------------------ // Figure out the kinetic energy stuff //------------------------------------ // temp = -1.0f - *elasticity; vn *= temp; vp.AddScaled(vn, worldLinearVelocity, 2.0f); // // Reflect the velocity vector // worldLinearVelocity += delta_v; temp = penetration * delta_t; delta_v *= temp; localOrigin.linearPosition += delta_v; // // Compute the kinetic energy loss // Check_Fpu(); return -0.0005 * (vn * vp) * moverMass; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Scalar Mover::DynamicBounce( Mover *other, Scalar delta_t, Scalar penetration, const Normal &normal, Scalar *elasticity ) { Check(this); Check(other); Check(&normal); Check_Pointer(elasticity); Verify(penetration >= 0.0f && penetration <= 1.0f); Verify(*elasticity >= 0.0f && *elasticity <= 1.0f); Verify(delta_t > SMALL); // //------------------------------------------------------------------------ // Get the relative velocity of the other guy, and figure out the velocity // delta along the normal //------------------------------------------------------------------------ // Scalar k1 = worldLinearVelocity.LengthSquared(); Scalar k2 = other->worldLinearVelocity.LengthSquared(); Scalar mass_ratio = other->moverMass / (moverMass + other->moverMass); Check_Fpu(); Vector3D v; v.Subtract(other->worldLinearVelocity, worldLinearVelocity); Scalar temp = (1.0f + *elasticity) * (v*normal); Vector3D delta_v; delta_v.Multiply(normal, temp); // //------------------------------------------------------------------------- // Figure out the kinetic energy loss in kilojoules, and bounce the primary // mover // // There was an additional multiplication by mass ratio in system 3 code... // we should make sure it is really needed... //------------------------------------------------------------------------- // v.AddScaled(delta_v, v, -2.0f); worldLinearVelocity.AddScaled( worldLinearVelocity, delta_v, mass_ratio ); localOrigin.linearPosition.AddScaled( localOrigin.linearPosition, delta_v, delta_t * penetration ); // //---------------------------------------------------------- // Bounce the second object, and reset it's update values... //---------------------------------------------------------- // other->worldLinearVelocity.AddScaled( other->worldLinearVelocity, delta_v, mass_ratio - 1.0f ); other->localOrigin.linearPosition.AddScaled( other->localOrigin.linearPosition, delta_v, delta_t ); other->updateVelocity.linearMotion = other->worldLinearVelocity; other->updateOrigin.linearPosition = other->localOrigin.linearPosition; other->lastUpdate = Now(); // //-------------------------------- // Return the result in kilojoules //-------------------------------- // k1 -= worldLinearVelocity.LengthSquared(); k2 -= other->worldLinearVelocity.LengthSquared(); Check_Fpu(); return 0.0005f * mass_ratio * (moverMass * k1 + other->moverMass * k2); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::CheckAgainstBoxedSolidChain( BoxedSolidCollisionList *collisions, BoxedSolid *chain ) { Check(this); Check(collisions); // //---------------------------------------------------------------------- // If the two movers collided against with each other, add the result to // the collision list //---------------------------------------------------------------------- // while (chain) { Check(chain); ExtentBox slice; if (chain->Intersects(*collisionVolume, &slice)) { Verify(collisions->GetCollisionsLeft()); collisions->AddCollisionToList(chain, slice); if (!collisions->GetCollisionsLeft()) { return; } } chain = chain->GetNextSolid(); } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // void Mover::CheckVolumeAgainstBoxedSolidChain( BoxedSolidCollisionList *collisions, BoxedSolid *chain ) { Check(this); Check(collisions); // //---------------------------------------------------------------------- // If the two movers collided against with each other, add the result to // the collision list //---------------------------------------------------------------------- // while (chain) { Check(chain); ExtentBox slice; if (chain->Intersects(*collisionVolume, &slice)) { Verify(collisions->GetCollisionsLeft()); collisions->AddCollisionToList(collisionVolume, slice); if (!collisions->GetCollisionsLeft()) { return; } } chain = chain->GetNextSolid(); } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // BoxedSolid* Mover::CheckLineAgainstBoxedSolidChain( Line *line, BoxedSolid *chain ) { Check(this); Check(line); // //---------------------------------------------------------------------- // If the two movers collided against with each other, add the result to // the collision list //---------------------------------------------------------------------- // BoxedSolid *result = NULL; while (chain) { Check(chain); if (chain->HitBy(line)) { result = chain; } chain = chain->GetNextSolid(); } Check_Fpu(); return result; } //############################################################################# // Construction and Destruction // //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Mover::Mover( Mover::MakeMessage *creation_message, Mover::SharedData &virtual_data ): Entity(creation_message, virtual_data) { Check_Pointer(this); Check(creation_message); Check(application); ResourceFile *res_file = application->GetResourceFile(); Check(res_file); // //------------------------------ // Initialize the motion vectors //------------------------------ // localVelocity = creation_message->localVelocity; localAcceleration = creation_message->localAcceleration; worldLinearAcceleration.Multiply( localAcceleration.linearMotion, localToWorld ); worldLinearVelocity.Multiply( localVelocity.linearMotion, localToWorld ); updateVelocity.linearMotion = worldLinearVelocity; updateVelocity.angularMotion = localVelocity.linearMotion; updateAcceleration.linearMotion = worldLinearAcceleration; updateAcceleration.angularMotion = localAcceleration.linearMotion; nextUpdate = lastUpdate; ResetUpdateIntervals(); predictedInterval = 0.0f; predictionError = 0.0f; widestGap = 0.0f; longGapCount = 0; queuedGapCount = 0; haveSenderSample = False; senderStamp = lastUpdate; senderPosition = localOrigin.linearPosition; senderVelocity.x = 0.0f; senderVelocity.y = 0.0f; senderVelocity.z = 0.0f; normalizeCount = 0; if (IsInitialStasis()) { SetSimulationState(StasisState); } collisionVolume = NULL; collisionTemplate = NULL; containedByNode = NULL; collisionLists = NULL; lastCollisionList = NULL; collisionAssistant = NULL; deadReckoner = NULL; collisionVolumeCount = 0; ResourceDescription *res = res_file->SearchList( resourceID, ResourceDescription::GameModelResourceType ); Check(res); res->Lock(); ModelResource* model = (ModelResource*)res->resourceAddress; Check_Pointer(model); moverMass = model->moverMass; Verify(!Small_Enough(model->momentOfInertia.x)); momentOfInertia.x = 1.0f/model->momentOfInertia.x; Verify(!Small_Enough(model->momentOfInertia.y)); momentOfInertia.y = 1.0f/model->momentOfInertia.y; Verify(!Small_Enough(model->momentOfInertia.z)); momentOfInertia.z = 1.0f/model->momentOfInertia.z; positiveLinearDragCoefficients = model->positiveLinearDragCoefficients; negativeLinearDragCoefficients = model->negativeLinearDragCoefficients; angularDragCoefficients = model->angularDragCoefficients; frictionCoefficient = model->frictionCoefficient; elasticityCoefficient = model->elasticityCoefficient; minimumBounceSpeed = model->minimumBounceSpeed; // //-------------------------------------------------------------------- // Read the collision information from the resource file, but for now, // assume a VTV //-------------------------------------------------------------------- // collisionLists = new BoxedSolidCollisionList[2]; Register_Pointer(collisionLists); res->Unlock(); if (IsCollisionVolume()) { res = res_file->SearchList( resourceID, ResourceDescription::BoxedSolidStreamResourceType ); Check(res); res->Lock(); BoxedSolidResource* box = (BoxedSolidResource*)res->resourceAddress; Check_Pointer(box); collisionVolumeCount = res->resourceSize / sizeof(BoxedSolidResource); for (int i=0; iUnlock(); MoveCollisionVolume(); } Check_Fpu(); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Logical Mover::CreateMakeMessage( MakeMessage *creation_message, NotationFile *model_file, const ResourceDirectories *directories ) { Check(creation_message); Check(model_file); if (!Entity::CreateMakeMessage(creation_message, model_file, directories)) { return False; } creation_message->messageLength = sizeof(Mover::MakeMessage); creation_message->classToCreate = RegisteredClass::TrivialMoverClassID; // creation_message->instanceFlags = DefaultFlags; creation_message->localVelocity = Motion::Identity; creation_message->localAcceleration = Motion::Identity; Check_Fpu(); return True; } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // ResourceDescription::ResourceID Mover::CreateModelResource( ResourceFile *resource_file, const char* model_name, NotationFile *model_file, const ResourceDirectories *,//directories, ModelResource *model ) { Check(resource_file); Check_Pointer(model_name); Check(model_file); // //----------------------------------------------------------------------- // If we were not provided a buffer to write the model data into, we must // create it ourselves //----------------------------------------------------------------------- // ModelResource *local_model = model; if (!local_model) { local_model = new ModelResource; Register_Pointer(local_model); } // //----------------- // Read in the mass //----------------- // if (!model_file->GetEntry("gamedata", "MoverMass", &local_model->moverMass)) { std::cerr << model_name << " missing MoverMass!\n"; Dump_And_Die: if (!model) { Unregister_Pointer(local_model); delete local_model; } Check_Fpu(); return -1; } // //------------------------------ // Read in the moment of inertia //------------------------------ // const char* entry; if ( !model_file->GetEntry( "gamedata", "MomentOfInertia", &entry ) ) { std::cerr << model_name << " missing MomentOfInertia!\n"; goto Dump_And_Die; } sscanf( entry, "%f %f %f", &local_model->momentOfInertia.x, &local_model->momentOfInertia.y, &local_model->momentOfInertia.z ); // //------------------------------ // Read in the drag coefficients //------------------------------ // if ( !model_file->GetEntry( "gamedata", "PositiveLinearDragCoefficients", &entry ) ) { std::cerr << model_name << " missing PositiveLinearDragCoefficients!\n"; goto Dump_And_Die; } sscanf( entry, "%f %f %f", &local_model->positiveLinearDragCoefficients.x, &local_model->positiveLinearDragCoefficients.y, &local_model->positiveLinearDragCoefficients.z ); if ( !model_file->GetEntry( "gamedata", "NegativeLinearDragCoefficients", &entry ) ) { std::cerr << model_name << " missing NegativeLinearDragCoefficients!\n"; goto Dump_And_Die; } sscanf( entry, "%f %f %f", &local_model->negativeLinearDragCoefficients.x, &local_model->negativeLinearDragCoefficients.y, &local_model->negativeLinearDragCoefficients.z ); // //------------------------- // Read in the angular drag //------------------------- // if ( !model_file->GetEntry( "gamedata", "AngularDragCoefficients", &entry ) ) { std::cerr << model_name << " missing AngularDragCoefficients!\n"; goto Dump_And_Die; } sscanf( entry, "%f %f %f", &local_model->angularDragCoefficients.x, &local_model->angularDragCoefficients.y, &local_model->angularDragCoefficients.z ); // //--------------------- // Read in the friction //--------------------- // if ( !model_file->GetEntry( "gamedata", "FrictionCoefficient", &local_model->frictionCoefficient ) ) { std::cerr << model_name << " missing FrictionCoefficient!\n"; goto Dump_And_Die; } // //----------------------- // Read in the elasticity //----------------------- // if ( !model_file->GetEntry( "gamedata", "ElasticityCoefficient", &local_model->elasticityCoefficient ) ) { std::cerr << model_name << " missing ElasticityCoefficient!\n"; goto Dump_And_Die; } // //--------------------------------- // Read in the minimum bounce speed //--------------------------------- // if ( !model_file->GetEntry( "gamedata", "MinimumBounceSpeed", &local_model->minimumBounceSpeed ) ) { std::cerr << model_name << " missing MinimumBounceSpeed!\n"; goto Dump_And_Die; } // //------------------------------------------------------------------------- // If we created the model buffer, then we have the responsibility to write // it out to the resource file //------------------------------------------------------------------------- // if (!model) { ResourceDescription *new_res = resource_file->AddResource( model_name, ResourceDescription::GameModelResourceType, 1, ResourceDescription::Preload, local_model, sizeof(*local_model) ); Unregister_Pointer(local_model); delete local_model; Check(new_res); Check_Fpu(); return new_res->resourceID; } else { Check_Fpu(); return 0; } } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Mover* Mover::Make(Mover::MakeMessage *creation_message) { return new Mover(creation_message, DefaultData); } //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // Mover::~Mover() { Unregister_Pointer(collisionLists); delete[] collisionLists; if (IsCollisionVolume()) { BoxedSolid *box = collisionTemplate; while (box) { BoxedSolid *next_box = box->GetNextSolid(); Unregister_Object(box); delete box; box = next_box; } box = collisionVolume; while (box) { BoxedSolid *next_box = box->GetNextSolid(); Unregister_Object(box); delete box; box = next_box; } } if (collisionAssistant) { Unregister_Object(collisionAssistant); delete collisionAssistant; } Check_Fpu(); } Logical Mover::TestInstance() const { return IsDerivedFrom(*GetClassDerivations()); }