void DockingProbe::TimeStep(double simt, double simdt)
{
if (!FirstTimeStepDone) {
DoFirstTimeStep();
FirstTimeStepDone = true;
return;
}
if (UndockNextTimestep) {
UpdatePort(Dockparam[1] * 0.5, simdt);
OurVessel->Undock(ourPort);
UndockNextTimestep = false;
}
if (ExtendingRetracting > 0) {
if (Status >= DOCKINGPROBE_STATUS_EXTENDED) {
Status = DOCKINGPROBE_STATUS_EXTENDED;
ExtendingRetracting = 0;
Dockproc = DOCKINGPROBE_PROC_UNDOCKED;
OurVessel->Undocking(ourPort);
OurVessel->SetDockingProbeMesh();
} else {
Status += 0.33 * simdt;
}
} else if (ExtendingRetracting < 0) {
if (Status <= DOCKINGPROBE_STATUS_RETRACTED) {
Status = DOCKINGPROBE_STATUS_RETRACTED;
ExtendingRetracting = 0;
OurVessel->HaveHardDocked(ourPort);
OurVessel->SetDockingProbeMesh();
} else {
Status -= 0.33 * simdt;
}
}
if (Dockproc == DOCKINGPROBE_PROC_SOFTDOCKED) {
UpdatePort(Dockparam[1] * 0.5, simdt);
Dockproc = DOCKINGPROBE_PROC_HARDDOCKED;
} else if (Dockproc == DOCKINGPROBE_PROC_HARDDOCKED) {
if (Status > DOCKINGPROBE_STATUS_RETRACTED) {
UpdatePort(Dockparam[1] * 0.5 * Status / 0.9, simdt);
} else {
UpdatePort(_V(0,0,0), simdt);
Dockproc = DOCKINGPROBE_PROC_UNDOCKED;
}
}
// sprintf(oapiDebugString(), "Docked %d Status %.3f Dockproc %d ExtendingRetracting %d", (Docked ? 1 : 0), Status, Dockproc, ExtendingRetracting);
// Switching logic
if (OurVessel->DockingProbeExtdRelSwitch.IsUp() && IsPowered()) {
Extend();
} else if (OurVessel->DockingProbeExtdRelSwitch.IsDown()) {
if ((!OurVessel->DockingProbeRetractPrimSwitch.IsCenter() && OurVessel->DockProbeMnACircuitBraker.IsPowered() && OurVessel->PyroBusA.Voltage() > SP_MIN_DCVOLTAGE) ||
(!OurVessel->DockingProbeRetractSecSwitch.IsCenter() && OurVessel->DockProbeMnBCircuitBraker.IsPowered() && OurVessel->PyroBusB.Voltage() > SP_MIN_DCVOLTAGE)) {
int ActiveCharges = 0;
if (OurVessel->DockingProbeRetractPrimSwitch.IsUp()) ActiveCharges = ActiveCharges | DOCKINGPROBE_CHARGE_PRIM1;
if (OurVessel->DockingProbeRetractPrimSwitch.IsDown()) ActiveCharges = ActiveCharges | DOCKINGPROBE_CHARGE_PRIM2;
if (OurVessel->DockingProbeRetractSecSwitch.IsUp()) ActiveCharges = ActiveCharges | DOCKINGPROBE_CHARGE_SEC1;
if (OurVessel->DockingProbeRetractSecSwitch.IsDown()) ActiveCharges = ActiveCharges | DOCKINGPROBE_CHARGE_SEC2;
if ((ActiveCharges & RetractChargesUsed)!= ActiveCharges) Retract();
RetractChargesUsed = RetractChargesUsed | ActiveCharges;
// sprintf(oapiDebugString(), "Charge Used: P1%d P2%d S1%d S2%d", RetractChargesUsed & DOCKINGPROBE_CHARGE_PRIM1 , RetractChargesUsed & DOCKINGPROBE_CHARGE_PRIM2 , RetractChargesUsed & DOCKINGPROBE_CHARGE_SEC1 , RetractChargesUsed & DOCKINGPROBE_CHARGE_SEC2);
}
}
///
/// Begin Advanced Docking Code
///
if (DockingMethod > ADVANCED){
// Code that follows is largely lifted from Atlantis...
// Goal is to handle close proximity docking between a probe and drogue
VECTOR3 gdrgPos, gdrgDir, gprbPos, gprbDir, gvslPos, rvel, pos, dir, rot;
OurVessel->Local2Global (Dockparam[0],gprbPos); //converts probe location to global
OurVessel->GlobalRot (Dockparam[1],gprbDir); //rotates probe direction to global
// Search the complete vessel list for a grappling candidate.
// Not very scalable ...
for (DWORD i = 0; i < oapiGetVesselCount(); i++) {
OBJHANDLE hV = oapiGetVesselByIndex (i);
if (hV == OurVessel->GetHandle()) continue; // we don't want to grapple ourselves ...
oapiGetGlobalPos (hV, &gvslPos);
if (dist (gvslPos, gprbPos) < oapiGetSize (hV)) { // in range
VESSEL *v = oapiGetVesselInterface (hV);
DWORD nAttach = v->AttachmentCount (true);
for (DWORD j = 0; j < nAttach; j++) { // now scan all attachment points of the candidate
ATTACHMENTHANDLE hAtt = v->GetAttachmentHandle (true, j);
const char *id = v->GetAttachmentId (hAtt);
if (strncmp (id, "PADROGUE", 8)) continue; // attachment point not compatible
v->GetAttachmentParams (hAtt, pos, dir, rot);
v->Local2Global (pos, gdrgPos); // converts found drogue position to global
v->GlobalRot (dir, gdrgDir); // rotates found drogue direction to global
if (dist (gdrgPos, gprbPos) < COLLISION_DETECT_RANGE && DockingMethod == ADVANCEDPHYSICS) { // found one less than a meter away!
// Detect if collision has happend, if so, t will return intersection point along the probe line X(t) = gprbPos + t * gprbDir
double t = CollisionDetection(gprbPos, gprbDir, gdrgPos, gdrgDir);
// Calculate time of penetration according to current velocity
OurVessel->GetRelativeVel(hV, rvel);
// Determine resultant force
//APPLY rforce to DockingProbe Vessel, and APPLY -rforce to Drogue Vessel
return;
}
if (dist(gdrgPos, gprbPos) < CAPTURE_DETECT_RANGE && DockingMethod > ADVANCED) {
// If we're within capture range, set docking port to attachment so docking can take place
// Originally, I would have used the Attachment features to soft dock and move the LM during retract
// but Artlav's docking method does this better and uses the docking port itself.
// Attachment is being used as a placeholder for the docking port and to identify its orientation.
OurVessel->GetAttachmentParams(hattPROBE, pos, dir, rot);
DOCKHANDLE dock = OurVessel->GetDockHandle(ourPort);
OurVessel->SetDockParams(dock, pos, dir, rot);
}
}//for nAttach
}//if inRange
}//for nVessel
}
}
