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 } }