src/data/assembly/constraints.cpp - master - levovix/kittycad

src/data/assembly/constraints.cpp
690 строк22 KB
levovix
add<wip>: more detailed control over Transmission3dConstraint axies
24 фев 2026, 10:35
24 фев 2026, 10:35b07ef99
#include "./constraints.h"
#include "data/assembly/solid_clone.h"
#include "mb_axis3d.h"

#include <QPainter>
#include <QSvgRenderer>

#include <gcm_types.h>
#include <gcm_api.h>
#include <gcm_blackbox.h>
#include <gcm_manager.h>



data::AssemblyConstraint::AssemblyConstraint() {}

data::AssemblyConstraint::~AssemblyConstraint() {}



void data::AssemblyConstraint::registerInGcm(GCM_system gcmSystem) {}

void data::AssemblyConstraint::updateFromGcm(GCM_system gcmSystem) {}

GCM_geom data::AssemblyConstraint::gcmIdAt(int index) { return _GCM_NULL; }



data::Fixed3dConstraint::Fixed3dConstraint() {}

data::Fixed3dConstraint::~Fixed3dConstraint() {}

void data::Fixed3dConstraint::draw(DrawContext& ctx) {
  // ctx.p->setPen(QPen(QBrush("#EB7964"), 6));
  
  if (this->assembly.expired()) { return; }
  auto assembly = this->assembly.lock();
  auto gcmSystem = assembly->gcmSystem;

  GCM_geom p1 = item.id();
  if (p1 == _GCM_NULL) { return; }
  
  Point3 p1_pos = GCM_Placement(gcmSystem, p1).GetOrigin();
  QPointF center = ctx.point_worldToQScreen(p1_pos);
  
  QSvgRenderer iconRenderer(QString(":/resources/icons/constraint-3d-lock.svg"));
  iconRenderer.render(ctx.p, QRectF(center - QPointF(8, 8), QSizeF(16, 16)));
}

void data::Fixed3dConstraint::registerInGcm(GCM_system gcmSystem) {
  std::array<GCM_geom, 1> items = {
    item.id(),
  };
  if (std::ranges::any_of(items, [](auto const& x) { return x == _GCM_NULL; })) { return; }

  GCM_FreezeGeom(gcmSystem, item.id());
}

GCM_geom data::Fixed3dConstraint::gcmIdAt(int index) {
  if (index == 0) { return gcmId; }
  else { return _GCM_NULL; }
}

std::string_view data::Fixed3dConstraint::iconName() const {
  return "tool_fixed";
}



data::Coinsidence3dConstraint::Coinsidence3dConstraint() {}

data::Coinsidence3dConstraint::~Coinsidence3dConstraint() {}

void data::Coinsidence3dConstraint::draw(DrawContext& ctx) {
  ctx.p->setPen(QPen(QBrush("#EB7964"), 6));
  
  if (this->assembly.expired()) { return; }
  auto assembly = this->assembly.lock();
  auto gcmSystem = assembly->gcmSystem;
  
  // todo
}

void data::Coinsidence3dConstraint::registerInGcm(GCM_system gcmSystem) {
  std::array<GCM_geom, 2> items = {
    item1.id(),
    item2.id(),
  };
  if (std::ranges::any_of(items, [](auto const& x) { return x == _GCM_NULL; })) { return; }

  gcmId = GCM_AddBinConstraint(
    gcmSystem, GCM_COINCIDENT,
    item1.id(), item2.id(),
    inverseDirection ? GCM_OPPOSITE : GCM_COORIENTED
  );
}

GCM_geom data::Coinsidence3dConstraint::gcmIdAt(int index) {
  if (index == 0) { return gcmId; }
  else { return _GCM_NULL; }
}

std::string_view data::Coinsidence3dConstraint::iconName() const {
  return "tool_coinsidence_3d";
}



struct Transmission3dConstraintImpl : MbRefItem, ItGCBlackbox {
  Transmission3dConstraintImpl();
  ~Transmission3dConstraintImpl();

  ItGeom* item1{};
  ItGeom* item2{};
  double k{1.0 / 2.0};
  Vec3 axis1{0, 0, 1};
  Vec3 axis2{0, 0, 1};
  Vec3 origin1{0, 0, 0};
  Vec3 origin2{0, 0, 0};
  Transmission3dConstraintImpl* reversed{};
  std::string name{};
  int callCount{0};

  bool isOn{true};
  MbPlacement3D prevItem1Placement{};
  MbPlacement3D prevItem2Placement{};

  void CollectMyInGeoms(IFC_Array<ItGeom>& res) const override;
  void CollectMyOutGeoms(IFC_Array<ItGeom>& res) const override;

  GCM_dependent_result CalculateDependent(std::vector<GeomPlace> const& inGeomPlaces, GeomPlace& depPlace) const override;
  bool IsMyOutGeom(ItGeom const&) const override;
  void FinishBlackBox()  override;

public:
  refcount_t  AddRef() const override { return MbRefItem::AddRef(); }
  refcount_t  Release() const override { return MbRefItem::Release(); }
};

struct RackAndPinion3dConstraintImpl : MbRefItem, ItGCBlackbox {
  RackAndPinion3dConstraintImpl();
  ~RackAndPinion3dConstraintImpl();

  ItGeom* item1{};
  ItGeom* item2{};
  double k{1.0 / 1.0};
  Vec3 axis{0, 0, 1};
  double R{10.0};          // pitch radius
  Vec3 rackDir{1, 0, 0};   // направление движения рейки (в мире), должно быть нормировано
  bool inverseDirection{false};
  RackAndPinion3dConstraintImpl* reversed{};
  std::string name{};
  int callCount{0};

  bool isOn{true};
  mutable MbPlacement3D prevItem1Placement{};
  mutable MbPlacement3D prevItem2Placement{};

  enum Mode { GearDrivesRack, RackDrivesGear };
  Mode mode{GearDrivesRack};


  void CollectMyInGeoms(IFC_Array<ItGeom>& res) const override;
  void CollectMyOutGeoms(IFC_Array<ItGeom>& res) const override;

  GCM_dependent_result CalculateDependent(std::vector<GeomPlace> const& inGeomPlaces, GeomPlace& depPlace) const override;
  bool IsMyOutGeom(ItGeom const&) const override;
  void FinishBlackBox()  override;

public:
  refcount_t  AddRef() const override { return MbRefItem::AddRef(); }
  refcount_t  Release() const override { return MbRefItem::Release(); }
};




void disableAllReversedConstraintsStartingWith(ref<data::SolidClone> start) {
  std::vector<ref<data::SolidClone>> next;
  
  auto assembly = start->parentAssembly.lock();
  for (auto cns_untyped : assembly->constraints) {
    if (auto cns = std::dynamic_pointer_cast<data::Transmission3dConstraint>(cns_untyped)) {
      if (cns->item1.obj.expired() || cns->item2.obj.expired()) { continue; }
      
      auto item1 = cns->item1.obj.lock();
      auto item2 = cns->item2.obj.lock();

      auto fwd = cns->impl_fwd.static_cast_to<Transmission3dConstraintImpl>();
      auto bck = cns->impl_bck.static_cast_to<Transmission3dConstraintImpl>();

      if (item2.get() == start.get()) {
        std::swap(item1, item2);
        std::swap(fwd, bck);
      }
      
      if (item1.get() != start.get()) {
        continue;
      }

      if (!fwd->isOn || !bck->isOn) {
        continue;
      }

      bck->isOn = false;
      
      if (!std::ranges::contains(next, item2.get(), [](auto const& x) { return x.get(); })) {
        next.push_back(item2);
      }
    }
    if (auto cns = std::dynamic_pointer_cast<data::RackAndPinion3dConstraint>(cns_untyped)) {
      if (cns->item1.obj.expired() || cns->item2.obj.expired()) { continue; }
      
      auto item1 = cns->item1.obj.lock();
      auto item2 = cns->item2.obj.lock();

      auto fwd = cns->impl_fwd.static_cast_to<RackAndPinion3dConstraintImpl>();
      auto bck = cns->impl_bck.static_cast_to<RackAndPinion3dConstraintImpl>();

      if (item2.get() == start.get()) {
        std::swap(item1, item2);
        std::swap(fwd, bck);
      }
      
      if (item1.get() != start.get()) {
        continue;
      }

      if (!fwd->isOn || !bck->isOn) {
        continue;
      }

      bck->isOn = false;
      
      if (!std::ranges::contains(next, item2.get(), [](auto const& x) { return x.get(); })) {
        next.push_back(item2);
      }
    }
  }

  for (auto const& x : next) {
    disableAllReversedConstraintsStartingWith(x);
  }
}

void addBlackBoxes(ref<data::AssemblyConstraint> cns, MtGeomSolver* solver) {
  if (auto ent = std::dynamic_pointer_cast<data::Transmission3dConstraint>(cns)) {
    auto fwd = ent->impl_fwd.static_cast_to<Transmission3dConstraintImpl>();
    auto bck = ent->impl_bck.static_cast_to<Transmission3dConstraintImpl>();
    
    if (fwd->isOn) { solver->AddBlackbox(*fwd); }
    if (bck->isOn) { solver->AddBlackbox(*bck); }
  }
   if (auto ent = std::dynamic_pointer_cast<data::RackAndPinion3dConstraint>(cns)) {
    auto fwd = ent->impl_fwd.static_cast_to<RackAndPinion3dConstraintImpl>();
    auto bck = ent->impl_bck.static_cast_to<RackAndPinion3dConstraintImpl>();
    
    if (fwd->isOn) { solver->AddBlackbox(*fwd); }
    if (bck->isOn) { solver->AddBlackbox(*bck); }
  }
}

void addTemporary(ptr<data::Assembly> assembly, ptr<data::C3dObject> child) {
  assembly.lock()->addTemporary(child.lock());
}



data::Transmission3dConstraint::Transmission3dConstraint() {}

data::Transmission3dConstraint::~Transmission3dConstraint() {}

void data::Transmission3dConstraint::draw(DrawContext& ctx) {
  ctx.p->setPen(QPen(QBrush("#EB7964"), 6));
  
  if (this->assembly.expired()) { return; }
  auto assembly = this->assembly.lock();
  auto gcmSystem = assembly->gcmSystem;
  
  // todo
}

void data::Transmission3dConstraint::updateFromParams() {
  assert(m_propertyChanged);
  m_propertyChanged = false;
  
  // после изменения параметров (в том числе после создания или загрузке документа)
  // сохраним текущие положения зависимых от нас объектов, чтобы потом сравнивать новое состояние с предыдущим
  if (!item1.obj.expired()) {
    prevItem1Placement = item1.obj.lock()->transformation();
    prevItem1Placement.SetOrigin(prevItem1Placement.GetOrigin() + origin1);
  }
  if (!item2.obj.expired()) {
    prevItem2Placement = item2.obj.lock()->transformation();
    prevItem2Placement.SetOrigin(prevItem2Placement.GetOrigin() + origin2);
  }
  
  AssemblyConstraint::updateFromParams();
}

void data::Transmission3dConstraint::updateFromDependencies() {
  m_dependencyChanged = false;
  if (m_propertyChanged) {
    updateFromParams();
  }
  else {
    AssemblyConstraint::updateFromDependencies();
  }
}

void data::Transmission3dConstraint::registerInGcm(GCM_system gcmSystem) {
  if (item1.id() == _GCM_NULL) { return; }
  if (item2.id() == _GCM_NULL) { return; }

  auto impl_fwd = SPtr(new Transmission3dConstraintImpl);
  impl_fwd->k = k2 / k1 * (inverseDirection? -1 : 1);
  impl_fwd->item1 = GCM_GeomItem(gcmSystem, item1.id());
  impl_fwd->item2 = GCM_GeomItem(gcmSystem, item2.id());
  impl_fwd->axis1 = axis1;
  impl_fwd->axis2 = axis2;
  impl_fwd->prevItem1Placement = prevItem1Placement;
  impl_fwd->prevItem2Placement = prevItem2Placement;
  impl_fwd->origin1 = origin1;
  impl_fwd->origin2 = origin2;
  impl_fwd->name = item1.obj.lock()->name() + " -> " + item2.obj.lock()->name();
  this->impl_fwd = impl_fwd.static_cast_to<MbRefItem>();
  
  auto impl_bck = SPtr(new Transmission3dConstraintImpl);
  impl_bck->k = k1 / k2 * (inverseDirection? -1 : 1);
  impl_bck->item1 = GCM_GeomItem(gcmSystem, item2.id());
  impl_bck->item2 = GCM_GeomItem(gcmSystem, item1.id());
  impl_fwd->axis1 = axis2;
  impl_fwd->axis2 = axis1;
  impl_bck->prevItem1Placement = prevItem2Placement;
  impl_bck->prevItem2Placement = prevItem1Placement;
  impl_bck->origin1 = origin2;
  impl_bck->origin2 = origin1;
  impl_bck->name = item1.obj.lock()->name() + " <- " + item2.obj.lock()->name();
  this->impl_bck = impl_bck.static_cast_to<MbRefItem>();

  impl_fwd->reversed = impl_bck;
  impl_bck->reversed = impl_fwd;
}

void data::Transmission3dConstraint::updateFromGcm(GCM_system gcmSystem) {
  if (!item1.obj.expired()) {
    prevItem1Placement = item1.obj.lock()->transformation();
  }
  if (!item2.obj.expired()) {
    prevItem2Placement = item2.obj.lock()->transformation();
  }

  auto fwd = impl_fwd.static_cast_to<Transmission3dConstraintImpl>();
  auto bck = impl_bck.static_cast_to<Transmission3dConstraintImpl>();
  
  #ifdef DEBUG_CONSTRAINTS
    printf("called %s %d times (%s)\n",
      fwd->name.c_str(),
      fwd->callCount,
      fwd->isOn ? "on" : "off"
    );

    printf("called %s %d times (%s)\n",
      bck->name.c_str(),
      bck->callCount,
      bck->isOn ? "on" : "off"
    );
  #endif
  
  fwd->isOn = true;
  bck->isOn = true;

  AssemblyConstraint::updateFromGcm(gcmSystem);
}

GCM_geom data::Transmission3dConstraint::gcmIdAt(int index) {
  if (index == 0) { return gcmId; }
  else { return _GCM_NULL; }
}

void data::Transmission3dConstraint::setDirection(bool forward) {
  impl_fwd.static_cast_to<Transmission3dConstraintImpl>()->isOn = forward;
  impl_bck.static_cast_to<Transmission3dConstraintImpl>()->isOn = !forward;
}

std::string_view data::Transmission3dConstraint::iconName() const {
  return "tool_transmission";
}



Transmission3dConstraintImpl::Transmission3dConstraintImpl() {}
Transmission3dConstraintImpl::~Transmission3dConstraintImpl() {}

void Transmission3dConstraintImpl::CollectMyInGeoms(IFC_Array<ItGeom>& res) const {
  if (!isOn) { return; }
  reversed->isOn = false;
  res.Add(item1);
}

void Transmission3dConstraintImpl::CollectMyOutGeoms(IFC_Array<ItGeom>& res) const {
  if (!isOn) { return; }
  reversed->isOn = false;
  res.Add(item2);
}

GCM_dependent_result Transmission3dConstraintImpl::CalculateDependent(
  std::vector<GeomPlace> const& inGeomPlaces,
  GeomPlace& depPlace
) const {
  ++const_cast<Transmission3dConstraintImpl*>(this)->callCount;
  if (!isOn) { return GCM_DEP_RESULT_Ok; }
  // reversed->isOn = false;

  MbPlacement3D item1Placement = inGeomPlaces[0].second;
  MbPlacement3D item2Placement = depPlace.second;
  item1Placement.SetOrigin(item1Placement.GetOrigin() + origin1);
  item2Placement.SetOrigin(item2Placement.GetOrigin() + origin2);

  double i1dAngle = 0;

  {
    auto Vx = MbPlacement3D{Point3{}, axis1}.GetAxisX();
    auto Vy = MbPlacement3D{Point3{}, axis1}.GetAxisY();
    auto X = Vx.x * item1Placement.GetAxisX() + Vx.y * item1Placement.GetAxisY() + Vx.z * item1Placement.GetAxisZ();
    auto prevX = Vx.x * prevItem1Placement.GetAxisX() + Vx.y * prevItem1Placement.GetAxisY() + Vx.z * prevItem1Placement.GetAxisZ();
    auto prevY = Vy.x * prevItem1Placement.GetAxisX() + Vy.y * prevItem1Placement.GetAxisY() + Vy.z * prevItem1Placement.GetAxisZ();
    i1dAngle = atan2(X * prevY, X * prevX);
  }

  double i2dAngle = 0;

  {
    auto Vx = MbPlacement3D{Point3{}, axis2}.GetAxisX();
    auto Vy = MbPlacement3D{Point3{}, axis2}.GetAxisY();
    auto X = Vx.x * item2Placement.GetAxisX() + Vx.y * item2Placement.GetAxisY() + Vx.z * item2Placement.GetAxisZ();
    auto prevX = Vx.x * prevItem2Placement.GetAxisX() + Vx.y * prevItem2Placement.GetAxisY() + Vx.z * prevItem2Placement.GetAxisZ();
    auto prevY = Vy.x * prevItem2Placement.GetAxisX() + Vy.y * prevItem2Placement.GetAxisY() + Vy.z * prevItem2Placement.GetAxisZ();
    i2dAngle += atan2(X * prevY, X * prevX);
  }

  double axial_d_angle = 0;

  {
    auto Vold = (prevItem2Placement.GetOrigin() - prevItem1Placement.GetOrigin()).GetNormalized();
    auto Vnew = (item2Placement.GetOrigin() - item1Placement.GetOrigin()).GetNormalized();
    auto VoldY = Vold;
    VoldY.Rotate(axis2, M_PI/2);
    auto x = lengthOnAxis(Vnew, Vold);
    auto y = lengthOnAxis(Vnew, VoldY);
    axial_d_angle = atan2(y, x);
  }

  auto rotAngle = i1dAngle * k - i2dAngle + axial_d_angle;
  
  depPlace.second.Rotate(MbAxis3D{item2Placement.GetOrigin(), axis2}, rotAngle);
  
  return GCM_DEP_RESULT_Ok;
}

bool Transmission3dConstraintImpl::IsMyOutGeom(ItGeom const& g) const {
  if (!isOn) { return false; }
  // reversed->isOn = false;
  return g.objectId == item2->objectId;
}

void Transmission3dConstraintImpl::FinishBlackBox() {
  
}



data::RackAndPinion3dConstraint::RackAndPinion3dConstraint() {}

data::RackAndPinion3dConstraint::~RackAndPinion3dConstraint() {}

void data::RackAndPinion3dConstraint::draw(DrawContext& ctx) {
  ctx.p->setPen(QPen(QBrush("#EB7964"), 6));
  
  if (this->assembly.expired()) { return; }
  auto assembly = this->assembly.lock();
  auto gcmSystem = assembly->gcmSystem;
  
  // todo
}

void data::RackAndPinion3dConstraint::updateFromParams() {
  assert(m_propertyChanged);
  m_propertyChanged = false;
  
  // после изменения параметров (в том числе после создания или загрузке документа)
  // сохраним текущие положения зависимых от нас объектов, чтобы потом сравнивать новое состояние с предыдущим
  if (!item1.obj.expired()) {
    prevItem1Placement = item1.obj.lock()->transformation();
  }
  if (!item2.obj.expired()) {
    prevItem2Placement = item2.obj.lock()->transformation();
  }
  
  AssemblyConstraint::updateFromParams();
}

void data::RackAndPinion3dConstraint::updateFromDependencies() {
  m_dependencyChanged = false;
  if (m_propertyChanged) {
    updateFromParams();
  }
  else {
    AssemblyConstraint::updateFromDependencies();
  }
}

void data::RackAndPinion3dConstraint::registerInGcm(GCM_system gcmSystem) {
  if (item1.id() == _GCM_NULL) { return; }
  if (item2.id() == _GCM_NULL) { return; }

  auto impl_fwd = SPtr(new RackAndPinion3dConstraintImpl);
  impl_fwd->mode = RackAndPinion3dConstraintImpl::GearDrivesRack;
  impl_fwd->item1 = GCM_GeomItem(gcmSystem, item1.id()); // gear
  impl_fwd->item2 = GCM_GeomItem(gcmSystem, item2.id()); // rack
  impl_fwd->axis = axis;
  impl_fwd->R = R;
  impl_fwd->rackDir = rackDir;
  impl_fwd->inverseDirection = inverseDirection;
  impl_fwd->prevItem1Placement = prevItem1Placement;  // prev gear
  impl_fwd->prevItem2Placement = prevItem2Placement;  // prev rack
  this->impl_fwd = impl_fwd.static_cast_to<MbRefItem>();

  auto impl_bck = SPtr(new RackAndPinion3dConstraintImpl);
  impl_bck->mode = RackAndPinion3dConstraintImpl::RackDrivesGear;
  impl_bck->item1 = GCM_GeomItem(gcmSystem, item2.id()); // rack
  impl_bck->item2 = GCM_GeomItem(gcmSystem, item1.id()); // gear
  impl_bck->axis = axis;
  impl_bck->R = R;
  impl_bck->rackDir = rackDir;
  impl_bck->inverseDirection = inverseDirection;
  impl_bck->prevItem1Placement = prevItem2Placement;  // prev rack
  impl_bck->prevItem2Placement = prevItem1Placement;  // prev gear
  this->impl_bck = impl_bck.static_cast_to<MbRefItem>();

  impl_fwd->reversed = impl_bck;
  impl_bck->reversed = impl_fwd;
}

void data::RackAndPinion3dConstraint::updateFromGcm(GCM_system gcmSystem) {
  if (!item1.obj.expired()) {
    prevItem1Placement = item1.obj.lock()->transformation();
  }
  if (!item2.obj.expired()) {
    prevItem2Placement = item2.obj.lock()->transformation();
  }

  auto fwd = impl_fwd.static_cast_to<RackAndPinion3dConstraintImpl>();
  auto bck = impl_bck.static_cast_to<RackAndPinion3dConstraintImpl>();
  
  #ifdef DEBUG_CONSTRAINTS
    printf("called %s %d times (%s)\n",
      fwd->name.c_str(),
      fwd->callCount,
      fwd->isOn ? "on" : "off"
    );

    printf("called %s %d times (%s)\n",
      bck->name.c_str(),
      bck->callCount,
      bck->isOn ? "on" : "off"
    );
  #endif
  
  fwd->isOn = true;
  bck->isOn = true;

  AssemblyConstraint::updateFromGcm(gcmSystem);
}

GCM_geom data::RackAndPinion3dConstraint::gcmIdAt(int index) {
  if (index == 0) { return gcmId; }
  else { return _GCM_NULL; }
}

void data::RackAndPinion3dConstraint::setDirection(bool forward) {
  impl_fwd.static_cast_to<RackAndPinion3dConstraintImpl>()->isOn = forward;
  impl_bck.static_cast_to<RackAndPinion3dConstraintImpl>()->isOn = !forward;
}

std::string_view data::RackAndPinion3dConstraint::iconName() const {
  return "tool_rack_and_pinion";
}



RackAndPinion3dConstraintImpl::RackAndPinion3dConstraintImpl() {
  
}

RackAndPinion3dConstraintImpl::~RackAndPinion3dConstraintImpl() {
  
}

void RackAndPinion3dConstraintImpl::CollectMyInGeoms(IFC_Array<ItGeom>& res) const {
  if (!isOn) { return; }
  // reversed->isOn = false;
  res.Add(item1);
}

void RackAndPinion3dConstraintImpl::CollectMyOutGeoms(IFC_Array<ItGeom>& res) const {
  if (!isOn) { return; }
  // reversed->isOn = false;
  res.Add(item2);
}

GCM_dependent_result RackAndPinion3dConstraintImpl::CalculateDependent(
  std::vector<GeomPlace> const& inGeomPlaces,
  GeomPlace& depPlace
) const {
  ++const_cast<RackAndPinion3dConstraintImpl*>(this)->callCount;
  if (!isOn) return GCM_DEP_RESULT_Ok;

  const double sign = inverseDirection ? -1.0 : 1.0;

  MbVector3D dir(rackDir.x, rackDir.y, rackDir.z);
  if (!dir.Normalize()) return GCM_DEP_RESULT_Ok; // защита от нулевого вектора

  if (mode == Mode::GearDrivesRack) {
    MbPlacement3D gearPlacement = inGeomPlaces[0].second;
    MbPlacement3D rackPlacement = depPlace.second;
    double dTheta = 0.0;
    {
      auto Vx = MbPlacement3D{Point3{}, axis}.GetAxisX();
      auto Vy = MbPlacement3D{Point3{}, axis}.GetAxisY();

      auto X     = Vx.x * gearPlacement.GetAxisX()      + Vx.y * gearPlacement.GetAxisY()      + Vx.z * gearPlacement.GetAxisZ();
      auto prevX = Vx.x * prevItem1Placement.GetAxisX() + Vx.y * prevItem1Placement.GetAxisY() + Vx.z * prevItem1Placement.GetAxisZ();
      auto prevY = Vy.x * prevItem1Placement.GetAxisX() + Vy.y * prevItem1Placement.GetAxisY() + Vy.z * prevItem1Placement.GetAxisZ();

      dTheta = atan2(X * prevY, X * prevX);
    }
    depPlace.second.SetOrigin(depPlace.second.GetOrigin() - projectToAxis(depPlace.second.GetOrigin() - prevItem2Placement.GetOrigin(), rackDir));
    
    double ds = sign * R * dTheta;
   
    depPlace.second.Move(rackDir * ds);

    return GCM_DEP_RESULT_Ok;
  }
  else { // Mode::RackDrivesGear
    MbPlacement3D rackPlacement = inGeomPlaces[0].second;
     MbPlacement3D gearPlacement = depPlace.second;

    // Нормализуем направление рейки
    MbVector3D dir(rackDir.x, rackDir.y, rackDir.z);
    if (!dir.Normalize()) return GCM_DEP_RESULT_Ok;

    // Считаем ds как проекцию перемещения рейки на dir
    const auto prevRackO = prevItem1Placement.GetOrigin();
    const auto nowRackO  = rackPlacement.GetOrigin();

    const double ds = lengthOnAxis(nowRackO - prevRackO, rackDir);
    
    // Переводим ds в угол шестерни
    if (R == 0.0) return GCM_DEP_RESULT_Ok;
    double dTheta = sign * (ds / R);
    double dTheta1 = 0.0;
    {
      auto Vx = MbPlacement3D{Point3{}, axis}.GetAxisX();
      auto Vy = MbPlacement3D{Point3{}, axis}.GetAxisY();

      auto X     = Vx.x * gearPlacement.GetAxisX()      + Vx.y * gearPlacement.GetAxisY()      + Vx.z * gearPlacement.GetAxisZ();
      auto prevX = Vx.x * prevItem2Placement.GetAxisX() + Vx.y * prevItem2Placement.GetAxisY() + Vx.z * prevItem2Placement.GetAxisZ();
      auto prevY = Vy.x * prevItem2Placement.GetAxisX() + Vy.y * prevItem2Placement.GetAxisY() + Vy.z * prevItem2Placement.GetAxisZ();

      dTheta1 = atan2(X * prevY, X * prevX);
      dTheta -= dTheta1;
    }
    // Вращаем шестерню (depPlace) вокруг её оси, на месте
    depPlace.second.Rotate(MbAxis3D{depPlace.second.GetOrigin(), axis}, dTheta);
    
    return GCM_DEP_RESULT_Ok;
  }
}
  
bool RackAndPinion3dConstraintImpl::IsMyOutGeom(ItGeom const& g) const {
  if (!isOn) { return false; }
  // reversed->isOn = false;
  return g.objectId == item2->objectId;
}

void RackAndPinion3dConstraintImpl::FinishBlackBox() {
  
}