+ myAttributeMap[theEntity] = aResult;
+ theType = aCurrentEntity.type;
+ return aResult;
+}
+
+Slvs_hEntity SketchSolver_Constraint::changeEntity(FeaturePtr theEntity, int& theType)
+{
+ Slvs_hEntity aResult = SLVS_E_UNKNOWN;
+ if (!theEntity || !theEntity->data() || !theEntity->data()->isValid())
+ return SLVS_E_UNKNOWN;
+ // If the entity is already in the group, try to find it
+ std::map<FeaturePtr, Slvs_hEntity>::const_iterator anEntIter = myFeatureMap.find(theEntity);
+ Slvs_Entity aCurrentEntity;
+ aCurrentEntity.h = SLVS_E_UNKNOWN;
+ if (anEntIter != myFeatureMap.end())
+ aCurrentEntity = myStorage->getEntity(anEntIter->second);
+ else {
+ aResult = myGroup->getFeatureId(theEntity);
+ if (aResult != SLVS_E_UNKNOWN) {
+ Slvs_Entity anEnt = myStorage->getEntity(aResult);
+ theType = anEnt.type;
+ myFeatureMap[theEntity] = aResult;
+ return aResult;
+ }
+ }
+
+ Slvs_hGroup aGroupID = myGroup->getId();
+ Slvs_hEntity aWorkplaneID = myGroup->getWorkplaneId();
+ DataPtr aData = theEntity->data();
+
+ // SketchPlugin features
+ const std::string& aFeatureKind = theEntity->getKind();
+ AttributePtr anAttribute;
+ int anAttrType;
+ // Line
+ if (aFeatureKind == SketchPlugin_Line::ID()) {
+ anAttribute = aData->attribute(SketchPlugin_Line::START_ID());
+ if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
+ Slvs_hEntity aStart = changeEntity(anAttribute, anAttrType);
+
+ anAttribute = aData->attribute(SketchPlugin_Line::END_ID());
+ if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
+ Slvs_hEntity aEnd = changeEntity(anAttribute, anAttrType);
+
+ if (aCurrentEntity.h == SLVS_E_UNKNOWN) // New entity
+ aCurrentEntity = Slvs_MakeLineSegment(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID, aStart, aEnd);
+ else {
+ aCurrentEntity.point[0] = aStart;
+ aCurrentEntity.point[1] = aEnd;
+ }
+ aResult = myStorage->addEntity(aCurrentEntity);
+ }
+ // Circle
+ else if (aFeatureKind == SketchPlugin_Circle::ID()) {
+ anAttribute = aData->attribute(SketchPlugin_Circle::CENTER_ID());
+ if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
+ Slvs_hEntity aCenter = changeEntity(anAttribute, anAttrType);
+
+ anAttribute = aData->attribute(SketchPlugin_Circle::RADIUS_ID());
+ if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
+ Slvs_hEntity aRadius = changeEntity(anAttribute, anAttrType);
+
+ if (aCurrentEntity.h == SLVS_E_UNKNOWN) { // New entity
+ Slvs_Entity aWorkplane = myStorage->getEntity(aWorkplaneID);
+ aCurrentEntity = Slvs_MakeCircle(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID,
+ aCenter, aWorkplane.normal, aRadius);
+ } else {
+ aCurrentEntity.point[0] = aCenter;
+ aCurrentEntity.distance = aRadius;
+ }
+ aResult = myStorage->addEntity(aCurrentEntity);
+ }
+ // Arc
+ else if (aFeatureKind == SketchPlugin_Arc::ID()) {
+ anAttribute = aData->attribute(SketchPlugin_Arc::CENTER_ID());
+ if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
+ Slvs_hEntity aCenter = changeEntity(anAttribute, anAttrType);
+
+ anAttribute = aData->attribute(SketchPlugin_Arc::START_ID());
+ if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
+ Slvs_hEntity aStart = changeEntity(anAttribute, anAttrType);
+
+ anAttribute = aData->attribute(SketchPlugin_Arc::END_ID());
+ if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
+ Slvs_hEntity aEnd = changeEntity(anAttribute, anAttrType);
+
+ if (aCurrentEntity.h == SLVS_E_UNKNOWN) { // New entity
+ Slvs_Entity aWorkplane = myStorage->getEntity(aWorkplaneID);
+ aCurrentEntity = Slvs_MakeArcOfCircle(SLVS_E_UNKNOWN, aGroupID, aWorkplaneID,
+ aWorkplane.normal, aCenter, aStart, aEnd);
+ } else {
+ aCurrentEntity.point[0] = aCenter;
+ aCurrentEntity.point[1] = aStart;
+ aCurrentEntity.point[2] = aEnd;
+ }
+ aResult = myStorage->addEntity(aCurrentEntity);
+ }
+ // Point (it has low probability to be an attribute of constraint, so it is checked at the end)
+ else if (aFeatureKind == SketchPlugin_Point::ID()) {
+ anAttribute = aData->attribute(SketchPlugin_Point::COORD_ID());
+ if (!isInitialized(anAttribute)) return SLVS_E_UNKNOWN;
+ // Both the sketch point and its attribute (coordinates) link to the same SolveSpace point identifier
+ aResult = changeEntity(anAttribute, anAttrType);
+ aCurrentEntity.type = SLVS_E_POINT_IN_3D;
+ }
+
+ if (aResult != SLVS_E_UNKNOWN) {
+ myFeatureMap[theEntity] = aResult;
+ theType = aCurrentEntity.type;
+ }
+ return aResult;
+}
+
+std::list<ConstraintPtr> SketchSolver_Constraint::constraints() const
+{
+ std::list<ConstraintPtr> aConstraints;
+ aConstraints.push_back(myBaseConstraint);
+ return aConstraints;
+}
+
+void SketchSolver_Constraint::refresh()
+{
+ cleanErrorMsg();
+ std::map<AttributePtr, Slvs_hEntity>::iterator anAttrIter = myAttributeMap.begin();
+ for (; anAttrIter != myAttributeMap.end(); anAttrIter++) {
+ std::shared_ptr<GeomDataAPI_Point> aPoint =
+ std::dynamic_pointer_cast<GeomDataAPI_Point>(anAttrIter->first);
+ if (aPoint) {
+ Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
+ double aXYZ[3];
+ for (int i = 0; i < 3; i++) {
+ Slvs_Param aPar = myStorage->getParameter(anEntity.param[i]);
+ aXYZ[i] = aPar.val;
+ }
+ if (fabs(aPoint->x() - aXYZ[0]) > tolerance ||
+ fabs(aPoint->y() - aXYZ[1]) > tolerance ||
+ fabs(aPoint->z() - aXYZ[2]) > tolerance)
+ aPoint->setValue(aXYZ[0], aXYZ[1], aXYZ[2]);
+ } else {
+ // Point in 2D
+ std::shared_ptr<GeomDataAPI_Point2D> aPoint2D =
+ std::dynamic_pointer_cast<GeomDataAPI_Point2D>(anAttrIter->first);
+ if (aPoint2D) {
+ Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
+ double aXY[2];
+ for (int i = 0; i < 2; i++) {
+ Slvs_Param aPar = myStorage->getParameter(anEntity.param[i]);
+ aXY[i] = aPar.val;
+ }
+ if (fabs(aPoint2D->x() - aXY[0]) > tolerance ||
+ fabs(aPoint2D->y() - aXY[1]) > tolerance)
+ aPoint2D->setValue(aXY[0], aXY[1]);
+ } else {
+ // Scalar value (used for the distance entities)
+ AttributeDoublePtr aScalar =
+ std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anAttrIter->first);
+ if (aScalar) {
+ Slvs_Entity anEntity = myStorage->getEntity(anAttrIter->second);
+ Slvs_Param aPar = myStorage->getParameter(anEntity.param[0]);
+ if (fabs(aScalar->value() - aPar.val) > tolerance)
+ aScalar->setValue(aPar.val);
+ }
+ }
+ }
+ }
+
+ std::map<AttributePtr, Slvs_hParam>::iterator aValIter = myValueMap.begin();
+ for (; aValIter != myValueMap.end(); aValIter++) {
+ AttributeDoublePtr aScalar =
+ std::dynamic_pointer_cast<ModelAPI_AttributeDouble>(anAttrIter->first);
+ if (aScalar) {
+ Slvs_Param aPar = myStorage->getParameter(anAttrIter->second);
+ aScalar->setValue(aPar.val);
+ }
+ }
+}
+
+Slvs_hEntity SketchSolver_Constraint::getId(FeaturePtr theFeature) const
+{
+ std::map<FeaturePtr, Slvs_hEntity>::const_iterator aFIter = myFeatureMap.find(theFeature);
+ if (aFIter == myFeatureMap.end())
+ return SLVS_E_UNKNOWN;
+ // check the Feature is really in the storage
+ Slvs_Entity anEntity = myStorage->getEntity(aFIter->second);
+ if (anEntity.h == SLVS_E_UNKNOWN) {
+ // rebuild feature
+ int aType;
+ anEntity.h = const_cast<SketchSolver_Constraint*>(this)->changeEntity(aFIter->first, aType);
+ const_cast<SketchSolver_Constraint*>(this)->myFeatureMap[theFeature] = anEntity.h;
+ }
+ return anEntity.h;
+}
+
+Slvs_hEntity SketchSolver_Constraint::getId(AttributePtr theAttribute) const
+{
+ std::map<AttributePtr, Slvs_hEntity>::const_iterator anAttrIter = myAttributeMap.find(theAttribute);
+ if (anAttrIter == myAttributeMap.end())
+ return SLVS_E_UNKNOWN;
+ return anAttrIter->second;
+}
+
+bool SketchSolver_Constraint::isInitialized(AttributePtr theAttribute)
+{
+ if (theAttribute->isInitialized())
+ return true;
+ myErrorMsg = SketchSolver_Error::NOT_INITIALIZED();
+ return false;
+}
+
+
+void SketchSolver_Constraint::calculateMiddlePoint(
+ const Slvs_Entity& theEntity, double theCoeff, double& theX, double& theY) const
+{
+ if (theEntity.type == SLVS_E_LINE_SEGMENT) {
+ double aStartEndXY[2][2];
+ Slvs_Entity aPoint;
+ for (int i = 0; i < 2; i++) {
+ aPoint = myStorage->getEntity(theEntity.point[i]);
+ for (int j = 0; j < 2; j++)
+ aStartEndXY[i][j] = myStorage->getParameter(aPoint.param[j]).val;
+ }
+ theX = (1.0 - theCoeff) * aStartEndXY[0][0] + theCoeff * aStartEndXY[1][0];
+ theY = (1.0 - theCoeff) * aStartEndXY[0][1] + theCoeff * aStartEndXY[1][1];
+ } else if (theEntity.type == SLVS_E_ARC_OF_CIRCLE) {
+ double anArcPoint[3][2];
+ Slvs_Entity aPoint;
+ for (int i = 0; i < 3; i++) {
+ aPoint = myStorage->getEntity(theEntity.point[i]);
+ for (int j = 0; j < 2; j++)
+ anArcPoint[i][j] = myStorage->getParameter(aPoint.param[j]).val;
+ }
+ // project last point of arc on the arc
+ double x = anArcPoint[1][0] - anArcPoint[0][0];
+ double y = anArcPoint[1][1] - anArcPoint[0][1];
+ double aRad = sqrt(x*x + y*y);
+ x = anArcPoint[2][0] - anArcPoint[0][0];
+ y = anArcPoint[2][1] - anArcPoint[0][1];
+ double aNorm = sqrt(x*x + y*y);
+ if (aNorm >= tolerance) {
+ anArcPoint[2][0] = x * aRad / aNorm;
+ anArcPoint[2][1] = y * aRad / aNorm;
+ }
+ anArcPoint[1][0] -= anArcPoint[0][0];
+ anArcPoint[1][1] -= anArcPoint[0][1];
+ if (theCoeff < tolerance) {
+ theX = anArcPoint[0][0] + anArcPoint[1][0];
+ theY = anArcPoint[0][1] + anArcPoint[1][1];
+ return;
+ } else if (1 - theCoeff < tolerance) {
+ theX = anArcPoint[0][0] + anArcPoint[2][0];
+ theY = anArcPoint[0][1] + anArcPoint[2][1];
+ return;
+ }
+
+ std::shared_ptr<GeomAPI_Dir2d> aStartDir(new GeomAPI_Dir2d(anArcPoint[1][0], anArcPoint[1][1]));
+ std::shared_ptr<GeomAPI_Dir2d> aEndDir(new GeomAPI_Dir2d(anArcPoint[2][0], anArcPoint[2][1]));
+ double anAngle = aStartDir->angle(aEndDir);
+ if (anAngle < 0)
+ anAngle += 2.0 * PI;
+ anAngle *= theCoeff;
+ double aCos = cos(anAngle);
+ double aSin = sin(anAngle);
+ theX = anArcPoint[0][0] + anArcPoint[1][0] * aCos - anArcPoint[1][1] * aSin;
+ theY = anArcPoint[0][1] + anArcPoint[1][0] * aSin + anArcPoint[1][1] * aCos;
+ }
+}
+
+void SketchSolver_Constraint::makeTemporary() const
+{
+ std::vector<Slvs_hConstraint>::const_iterator anIt = mySlvsConstraints.begin();
+ for (; anIt != mySlvsConstraints.end(); anIt++)
+ myStorage->addTemporaryConstraint(*anIt);