From c5b5980da324416d95fdee382518b0a49483a43a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jacob=20St=C3=B8ren?= Date: Fri, 15 Jun 2018 14:23:27 +0200 Subject: [PATCH] #3049 Add SolveSpaceLib libslvs with an S-Curve test to ResInsight --- ApplicationCode/Adm/LicenseInformation.txt | 8 + .../UnitTests/CMakeLists_files.cmake | 1 + .../UnitTests/SolveSpaceSolver-Test.cpp | 334 +++++ CMakeLists.txt | 10 + ResInsightVersion.cmake | 4 + ThirdParty/libslvs/CMakeLists.txt | 56 + ThirdParty/libslvs/LICENSE | 675 ++++++++++ ThirdParty/libslvs/SolveSpaceSystem.cpp | 169 +++ ThirdParty/libslvs/constrainteq.cpp | 799 ++++++++++++ ThirdParty/libslvs/dsc.h | 578 ++++++++ ThirdParty/libslvs/entity.cpp | 875 +++++++++++++ ThirdParty/libslvs/expr.cpp | 917 +++++++++++++ ThirdParty/libslvs/expr.h | 140 ++ ThirdParty/libslvs/include/SolveSpaceSystem.h | 51 + ThirdParty/libslvs/include/slvs.h | 409 ++++++ ThirdParty/libslvs/lib.cpp | 272 ++++ ThirdParty/libslvs/platform/platform.h | 68 + ThirdParty/libslvs/platform/unixutil.cpp | 124 ++ ThirdParty/libslvs/polygon.h | 415 ++++++ ThirdParty/libslvs/render/render.h | 366 ++++++ ThirdParty/libslvs/resource.h | 109 ++ ThirdParty/libslvs/sketch.h | 926 +++++++++++++ ThirdParty/libslvs/solvespace.h | 935 +++++++++++++ ThirdParty/libslvs/srf/surface.h | 429 ++++++ ThirdParty/libslvs/system.cpp | 587 +++++++++ ThirdParty/libslvs/util.cpp | 1161 +++++++++++++++++ 26 files changed, 10418 insertions(+) create mode 100644 ApplicationCode/UnitTests/SolveSpaceSolver-Test.cpp create mode 100644 ThirdParty/libslvs/CMakeLists.txt create mode 100644 ThirdParty/libslvs/LICENSE create mode 100644 ThirdParty/libslvs/SolveSpaceSystem.cpp create mode 100644 ThirdParty/libslvs/constrainteq.cpp create mode 100644 ThirdParty/libslvs/dsc.h create mode 100644 ThirdParty/libslvs/entity.cpp create mode 100644 ThirdParty/libslvs/expr.cpp create mode 100644 ThirdParty/libslvs/expr.h create mode 100644 ThirdParty/libslvs/include/SolveSpaceSystem.h create mode 100644 ThirdParty/libslvs/include/slvs.h create mode 100644 ThirdParty/libslvs/lib.cpp create mode 100644 ThirdParty/libslvs/platform/platform.h create mode 100644 ThirdParty/libslvs/platform/unixutil.cpp create mode 100644 ThirdParty/libslvs/polygon.h create mode 100644 ThirdParty/libslvs/render/render.h create mode 100644 ThirdParty/libslvs/resource.h create mode 100644 ThirdParty/libslvs/sketch.h create mode 100644 ThirdParty/libslvs/solvespace.h create mode 100644 ThirdParty/libslvs/srf/surface.h create mode 100644 ThirdParty/libslvs/system.cpp create mode 100644 ThirdParty/libslvs/util.cpp diff --git a/ApplicationCode/Adm/LicenseInformation.txt b/ApplicationCode/Adm/LicenseInformation.txt index 94ff318884..aaf5777a81 100644 --- a/ApplicationCode/Adm/LicenseInformation.txt +++ b/ApplicationCode/Adm/LicenseInformation.txt @@ -464,3 +464,11 @@ CRAVA is a software package for seismic inversion and conditioning of This Source Code Form is subject to the terms of the Mozilla Public License v. 2.0. If a copy of the MPL was not distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +=============================================================================== + Notice for the SolveSpaceLib library libslvs +=============================================================================== + +Copyright 2008-2013 Jonathan Westhues. +SolveSpace and thus SolveSpaceLib is distributed under the terms of the GPL3 license. + diff --git a/ApplicationCode/UnitTests/CMakeLists_files.cmake b/ApplicationCode/UnitTests/CMakeLists_files.cmake index b73a7e658f..c0e628e837 100644 --- a/ApplicationCode/UnitTests/CMakeLists_files.cmake +++ b/ApplicationCode/UnitTests/CMakeLists_files.cmake @@ -44,6 +44,7 @@ ${CMAKE_CURRENT_LIST_DIR}/RifCaseRealizationParametersReader-Test.cpp ${CMAKE_CURRENT_LIST_DIR}/RigWellLogExtractor-Test.cpp ${CMAKE_CURRENT_LIST_DIR}/RifEclipseSummaryAddress-Test.cpp ${CMAKE_CURRENT_LIST_DIR}/RiaTimeHistoryCurveTools-Test.cpp +${CMAKE_CURRENT_LIST_DIR}/SolveSpaceSolver-Test.cpp ) list(APPEND CODE_HEADER_FILES diff --git a/ApplicationCode/UnitTests/SolveSpaceSolver-Test.cpp b/ApplicationCode/UnitTests/SolveSpaceSolver-Test.cpp new file mode 100644 index 0000000000..763c7ebe4c --- /dev/null +++ b/ApplicationCode/UnitTests/SolveSpaceSolver-Test.cpp @@ -0,0 +1,334 @@ +#include "gtest/gtest.h" + +#include "SolveSpaceSystem.h" +#include + +#include +#include + +#include "SolveSpaceSystem.h" + +/*----------------------------------------------------------------------------- +* Test calculation of an S-shape curve: +* Arc-Line-Arc with start and endpoint along with their tangents as input. +*---------------------------------------------------------------------------*/ +void example_S_Curve(double p1x, + double p1y, + double p1z, + double azi1, + double inc1, + double rad1, + double p2x, + double p2y, + double p2z, + double azi2, + double inc2, + double rad2) +{ + SolveSpaceSystem sys; + + Slvs_hGroup group1 = 1; + Slvs_hGroup group2 = 2; + + // Group 1, Fixed + // P1 + Slvs_hParam p_p1x = sys.addParam( Slvs_MakeParam(-1, group1, p1x) ); + Slvs_hParam p_p1y = sys.addParam( Slvs_MakeParam(-1, group1, p1y) ); + Slvs_hParam p_p1z = sys.addParam( Slvs_MakeParam(-1, group1, p1z) ); + + Slvs_hEntity e_P1 = sys.addEntity( Slvs_MakePoint3d(-1, group1, p_p1x, p_p1y, p_p1z) ); + + // PT1 + double pt1x = p1x + sin(azi1)*sin(inc1); + double pt1y = p1y + cos(azi1)*sin(inc1); + double pt1z = p1z - cos(inc1); + + Slvs_hParam p_pt1x = sys.addParam( Slvs_MakeParam(-1, group1, pt1x) ); + Slvs_hParam p_pt1y = sys.addParam( Slvs_MakeParam(-1, group1, pt1y) ); + Slvs_hParam p_pt1z = sys.addParam( Slvs_MakeParam(-1, group1, pt1z) ); + + Slvs_hEntity e_PT1 = sys.addEntity( Slvs_MakePoint3d(-1, group1, p_pt1x, p_pt1y, p_pt1z) ); + + // Tangent Line 1 + + Slvs_hEntity e_LT1 = sys.addEntity(Slvs_MakeLineSegment(-1, group1, SLVS_FREE_IN_3D, e_P1, e_PT1)); + + // P2 + Slvs_hParam p_p2x = sys.addParam( Slvs_MakeParam(-1, group1, p2x) ); + Slvs_hParam p_p2y = sys.addParam( Slvs_MakeParam(-1, group1, p2y) ); + Slvs_hParam p_p2z = sys.addParam( Slvs_MakeParam(-1, group1, p2z) ); + + Slvs_hEntity e_P2 = sys.addEntity( Slvs_MakePoint3d(-1, group1, p_p2x, p_p2y, p_p2z) ); + + // PT2 + double pt2x = p2x + sin(azi2)*sin(inc2); + double pt2y = p2y + cos(azi2)*sin(inc2); + double pt2z = p2z - cos(inc2); + + Slvs_hParam p_pt2x = sys.addParam( Slvs_MakeParam(-1, group1, pt2x) ); + Slvs_hParam p_pt2y = sys.addParam( Slvs_MakeParam(-1, group1, pt2y) ); + Slvs_hParam p_pt2z = sys.addParam( Slvs_MakeParam(-1, group1, pt2z) ); + + Slvs_hEntity e_PT2 = sys.addEntity( Slvs_MakePoint3d(-1, group1, p_pt2x, p_pt2y, p_pt2z) ); + + // Tangent Line 2 + + Slvs_hEntity e_LT2 = sys.addEntity(Slvs_MakeLineSegment(-1, group1, SLVS_FREE_IN_3D, e_P2, e_PT2)); + + // Plane1 + + double unitQw, unitQx, unitQy, unitQz; + Slvs_MakeQuaternion(1, 0, 0, + 0, 1, 0, + &unitQw, &unitQx, &unitQy, &unitQz); + + // Plane 1 + + Slvs_hParam p_Plane1Qw = sys.addParam( Slvs_MakeParam(-1, group2, unitQw) ); + Slvs_hParam p_Plane1Qx = sys.addParam( Slvs_MakeParam(-1, group2, unitQx) ); + Slvs_hParam p_Plane1Qy = sys.addParam( Slvs_MakeParam(-1, group2, unitQy) ); + Slvs_hParam p_Plane1Qz = sys.addParam( Slvs_MakeParam(-1, group2, unitQz)); + Slvs_hEntity e_Plane1Q = sys.addEntity( Slvs_MakeNormal3d(-1, group2, + p_Plane1Qw, + p_Plane1Qx, + p_Plane1Qy, + p_Plane1Qz)); + Slvs_hEntity e_Plane1 = sys.addEntity( Slvs_MakeWorkplane(-1, group2, e_P1, e_Plane1Q) ); + + Slvs_hConstraint c_PT1Plane1 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PT_IN_PLANE, + SLVS_FREE_IN_3D, + 0.0, + e_PT1, + -1, + e_Plane1, + -1)); + // Arc1 center + Slvs_hParam p_c1x = sys.addParam( Slvs_MakeParam(-1, group2, 10.0) ); // Needs a better guess + Slvs_hParam p_c1y = sys.addParam( Slvs_MakeParam(-1, group2, 2.0) ); + + Slvs_hEntity e_C1 = sys.addEntity( Slvs_MakePoint2d(-1, group2, e_Plane1, p_c1x, p_c1y) ); + + Slvs_hEntity e_LP1C1 = sys.addEntity(Slvs_MakeLineSegment(-1, group2, e_Plane1, e_P1, e_C1)); + + Slvs_hConstraint c_perpT1_LP1C1 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PERPENDICULAR, + e_Plane1, + 0.0, + -1, + -1, + e_LT1, + e_LP1C1)); + + Slvs_hConstraint c_dist_P1C1 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PT_PT_DISTANCE, + e_Plane1, + rad1, + e_P1, + e_C1, + -1, + -1)); + + // Arc1 end + + Slvs_hParam p_p11x = sys.addParam( Slvs_MakeParam(-1, group2, 2.0) ); // Needs a better guess: Perp on p_c1x/p_c1y + Slvs_hParam p_p11y = sys.addParam( Slvs_MakeParam(-1, group2, -10.0) ); + + Slvs_hEntity e_P11 = sys.addEntity( Slvs_MakePoint2d(-1, group2, e_Plane1, p_p11x, p_p11y) ); + + Slvs_hEntity e_LC1P11 = sys.addEntity(Slvs_MakeLineSegment(-1, group2, e_Plane1, e_C1, e_P11)); + + + Slvs_hConstraint c_dist_C1P11 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_EQUAL_LENGTH_LINES, + e_Plane1, + 0.0, + -1, + -1, + e_LP1C1, + e_LC1P11)); + + + + + + // Plane 2 + + Slvs_hParam p_Plane2Qw = sys.addParam( Slvs_MakeParam(-1, group2, unitQw) ); + Slvs_hParam p_Plane2Qx = sys.addParam( Slvs_MakeParam(-1, group2, unitQx) ); + Slvs_hParam p_Plane2Qy = sys.addParam( Slvs_MakeParam(-1, group2, unitQy) ); + Slvs_hParam p_Plane2Qz = sys.addParam( Slvs_MakeParam(-1, group2, unitQz)); + Slvs_hEntity e_Plane2Q = sys.addEntity( Slvs_MakeNormal3d(-1, group2, + p_Plane2Qw, + p_Plane2Qx, + p_Plane2Qy, + p_Plane2Qz)); + + Slvs_hEntity e_Plane2 = sys.addEntity( Slvs_MakeWorkplane(-1, group2, e_P2, e_Plane2Q) ); + + Slvs_hConstraint c_PT2Plane2 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PT_IN_PLANE, + SLVS_FREE_IN_3D, + 0.0, + e_PT2, + -1, + e_Plane2, + -1)); + + // Arc2 center + + Slvs_hParam p_c2x = sys.addParam( Slvs_MakeParam(-1, group2, 10.0) ); // Needs a better guess + Slvs_hParam p_c2y = sys.addParam( Slvs_MakeParam(-1, group2, 2.0) ); + + Slvs_hEntity e_C2 = sys.addEntity( Slvs_MakePoint2d(-1, group2, e_Plane2, p_c2x, p_c2y) ); + + Slvs_hEntity e_LP2C2 = sys.addEntity(Slvs_MakeLineSegment(-1, group2, e_Plane2, e_P2, e_C2)); + + Slvs_hConstraint c_perpT2_LP2C2 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PERPENDICULAR, + e_Plane2, + 0.0, + -1, + -1, + e_LT2, + e_LP2C2)); + + Slvs_hConstraint c_dist_P2C2 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PT_PT_DISTANCE, + e_Plane2, + rad2, + e_P2, + e_C2, + -1, + -1)); + + // Arc2 end + + Slvs_hParam p_p22x = sys.addParam( Slvs_MakeParam(-1, group2, 2.0) ); // Needs a better guess: Perp on p_c1x/p_c1y + Slvs_hParam p_p22y = sys.addParam( Slvs_MakeParam(-1, group2, -10.0) ); + + Slvs_hEntity e_P22 = sys.addEntity( Slvs_MakePoint2d(-1, group2, e_Plane2, p_p22x, p_p22y) ); + + Slvs_hEntity e_LC2P22 = sys.addEntity(Slvs_MakeLineSegment(-1, group2, e_Plane2, e_C2, e_P22)); + + + Slvs_hConstraint c_dist_C2P22 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_EQUAL_LENGTH_LINES, + e_Plane2, + 0.0, + -1, + -1, + e_LP2C2, + e_LC2P22)); + + + + auto solveResult = sys.solve(group2, true); + assert(solveResult == SolveSpaceSystem::RESULT_OKAY); + + // Connecting the two planes + + // Connecting line + Slvs_hEntity e_LP11P22 = sys.addEntity(Slvs_MakeLineSegment(-1, group2, SLVS_FREE_IN_3D, e_P11, e_P22)); + + // Perpendicular constraints + + Slvs_hConstraint c_perpC1P11_LP11P22 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PERPENDICULAR, + SLVS_FREE_IN_3D, + 0.0, + -1, + -1, + e_LC1P11, + e_LP11P22)); + + solveResult = sys.solve(group2, true); + assert(solveResult == SolveSpaceSystem::RESULT_OKAY); + + + Slvs_hConstraint c_perpC2P22_LP11P22 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PERPENDICULAR, + SLVS_FREE_IN_3D, + 0.0, + -1, + -1, + e_LC2P22, + e_LP11P22)); + + solveResult = sys.solve(group2, true); + assert(solveResult == SolveSpaceSystem::RESULT_OKAY); + + // P11, P22 in plane constraints + + Slvs_hConstraint c_P11InPlane2 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PT_IN_PLANE, + SLVS_FREE_IN_3D, + 0.0, + e_P11, + -1, + e_Plane2, + -1)); + + solveResult = sys.solve(group2, true); + assert(solveResult == SolveSpaceSystem::RESULT_OKAY); + + Slvs_hConstraint c_P22InPlane1 = sys.addConstr(Slvs_MakeConstraint(-1, + group2, + SLVS_C_PT_IN_PLANE, + SLVS_FREE_IN_3D, + 0.0, + e_P22, + -1, + e_Plane1, + -1)); + + + solveResult = sys.solve(group2, true); + assert(solveResult == SolveSpaceSystem::RESULT_OKAY); + + + // Circle Center, Plane normals, P11, P22 + + std::valarray v_C1 = sys.global3DPos(e_C1); + std::valarray v_C2 = sys.global3DPos(e_C2); + + std::valarray v_N1 = std::get<2>( sys.orientationMx(e_Plane1Q)); + std::valarray v_N2 = std::get<2>( sys.orientationMx(e_Plane2Q)); + + std::valarray v_P11 = sys.global3DPos(e_P11); + std::valarray v_P22 = sys.global3DPos(e_P22); + + std::cout << "C1: " << "[ " << v_C1[0] << ", " << v_C1[1] << ", " << v_C1[2] << " ]" << std::endl; + std::cout << "N1: " << "[ " << v_N1[0] << ", " << v_N1[1] << ", " << v_N1[2] << " ]" << std::endl; + std::cout << "P11: " << "[ " << v_P11[0] << ", " << v_P11[1] << ", " << v_P11[2] << " ]" << std::endl; + std::cout << "C2: " << "[ " << v_C2[0] << ", " << v_C2[1] << ", " << v_C2[2] << " ]" << std::endl; + std::cout << "N1: " << "[ " << v_N2[0] << ", " << v_N2[1] << ", " << v_N2[2] << " ]" << std::endl; + std::cout << "P22: " << "[ " << v_P22[0] << ", " << v_P22[1] << ", " << v_P22[2] << " ]" << std::endl; + +} + +#define M_PI 3.14159265358979323846 // pi +#define M_PI_2 1.57079632679489661923 // pi/2 +#define M_PI_4 0.785398163397448309616 // pi/4 + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +TEST(SolveSpaceSolverTest, SCurve) +{ + example_S_Curve(100, 100, 0, 0, M_PI_4, 12, + 100, 150, -1000, M_PI, M_PI_4, 12); + + +} diff --git a/CMakeLists.txt b/CMakeLists.txt index 85d449a798..46bf768177 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -292,6 +292,16 @@ list(APPEND THIRD_PARTY_LIBRARIES clipper ) +################################################################################ +# libslvs +################################################################################ + +set(SLVS_BUILD_SHARED OFF CACHE BOOL "Use static linkage for libslvs" FORCE) + +add_subdirectory(ThirdParty/libslvs) +list(APPEND THIRD_PARTY_LIBRARIES + libslvs +) ################################################################################ # Thirdparty libraries are put in ThirdParty solution folder diff --git a/ResInsightVersion.cmake b/ResInsightVersion.cmake index 4f922bd86e..5f07935d43 100644 --- a/ResInsightVersion.cmake +++ b/ResInsightVersion.cmake @@ -28,5 +28,9 @@ set(OPM_FLOWDIAGNOSTICS_APPLICATIONS_SHA "24ff768dc509b6c6bbd0121ef46a5932fae929 # This file was moved from opm-core to opm-parser october 2016 # sha for Units.hpp 9a679071dd0066236154852c39a9e0b3c3ac4873 +# https://github.com/JacobStoren/SolveSpaceLib +set(LIB_SLVS_SHA "c16d0e875f91e8e2e07842d71d37305c042c35e6") +# Which is based on https://github.com/solvespace/solvespace master from early march 2018 + set(STRPRODUCTVER ${RESINSIGHT_MAJOR_VERSION}.${RESINSIGHT_MINOR_VERSION}.${RESINSIGHT_PATCH_VERSION}${RESINSIGHT_VERSION_TEXT}${RESINSIGHT_DEV_VERSION}) diff --git a/ThirdParty/libslvs/CMakeLists.txt b/ThirdParty/libslvs/CMakeLists.txt new file mode 100644 index 0000000000..a78dddee70 --- /dev/null +++ b/ThirdParty/libslvs/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 2.8.12) + +# Platform utilities + +if(MSVC) + add_definitions(-D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS) +endif() + +option(SLVS_BUILD_SHARED "Build SolveSpaceLib as a .dll or .so" ON) + +if (${SLVS_BUILD_SHARED}) + set(SLVS_LIB_TYPE "SHARED") + set(SLVS_SHARED_LIB_DEFINE "-DSLVS_LIB_SHARED") +else() + set(SLVS_LIB_TYPE "STATIC") +endif() + +add_library(libslvs ${SLVS_LIB_TYPE} + constrainteq.cpp + dsc.h + entity.cpp + expr.h + expr.cpp + polygon.h + resource.h + sketch.h + solvespace.h + system.cpp + util.cpp + platform/platform.h + platform/unixutil.cpp + include/slvs.h + srf/surface.h + lib.cpp + include/SolveSpaceSystem.h + SolveSpaceSystem.cpp) + +if (${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU") + target_compile_options(libslvs + PRIVATE -Wno-missing-field-initializers + PUBLIC -std=c++11) +endif() + +target_compile_definitions(libslvs + PUBLIC ${SLVS_SHARED_LIB_DEFINE} + PRIVATE -DLIBRARY) + +target_include_directories(libslvs + PUBLIC ${CMAKE_CURRENT_LIST_DIR}/include + PRIVATE ${CMAKE_CURRENT_LIST_DIR} ) + +set_target_properties(libslvs PROPERTIES + PUBLIC_HEADER ${CMAKE_SOURCE_DIR}/include/slvs.h + VERSION ${solvespace_VERSION_MAJOR}.${solvespace_VERSION_MINOR} + SOVERSION 1) + diff --git a/ThirdParty/libslvs/LICENSE b/ThirdParty/libslvs/LICENSE new file mode 100644 index 0000000000..a737dcfed5 --- /dev/null +++ b/ThirdParty/libslvs/LICENSE @@ -0,0 +1,675 @@ + + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/ThirdParty/libslvs/SolveSpaceSystem.cpp b/ThirdParty/libslvs/SolveSpaceSystem.cpp new file mode 100644 index 0000000000..221fe3e196 --- /dev/null +++ b/ThirdParty/libslvs/SolveSpaceSystem.cpp @@ -0,0 +1,169 @@ + +#define EXPORT_DLL +#include "SolveSpaceSystem.h" +#include + + + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +SolveSpaceSystem::SolveSpaceSystem() + : m_paramsMemory (new std::vector ()) + , m_entityMemory (new std::vector ()) + , m_constraintMemory (new std::vector ()) + , m_failedConstrMemory(new std::vector()) + +{ + m_paramsMemory ->reserve(100); + m_entityMemory ->reserve(100); + m_constraintMemory->reserve(100); +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +Slvs_hParam SolveSpaceSystem::addParam(Slvs_Param parameter) +{ + + parameter.h = static_cast(m_paramsMemory->size()+1); + m_paramsMemory->push_back(parameter); + + m_slvsSystem.param = m_paramsMemory->data(); + m_slvsSystem.params = static_cast(m_paramsMemory->size()); + + return parameter.h; +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +Slvs_hEntity SolveSpaceSystem::addEntity(Slvs_Entity entity) +{ + entity.h = static_cast(m_entityMemory->size()+1); + m_entityMemory->push_back(entity); + + m_slvsSystem.entity = m_entityMemory->data(); + m_slvsSystem.entities = static_cast(m_entityMemory->size()); + + return entity.h; +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +Slvs_hConstraint SolveSpaceSystem::addConstr(Slvs_Constraint constr) +{ + constr.h = static_cast(m_constraintMemory->size()+1); + m_constraintMemory->push_back(constr); + + m_slvsSystem.constraint = m_constraintMemory->data(); + m_slvsSystem.constraints = static_cast(m_constraintMemory->size()); + + return constr.h; +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +SolveSpaceSystem::ResultStatus SolveSpaceSystem::solve(Slvs_hGroup groupId, bool reportFailedConstraints /*= true*/) +{ + m_failedConstrMemory->resize(m_constraintMemory->size()); + + m_slvsSystem.failed = m_failedConstrMemory->data(); + m_slvsSystem.faileds = static_cast(m_failedConstrMemory->size()); + + m_slvsSystem.calculateFaileds = reportFailedConstraints; + + Slvs_Solve(&m_slvsSystem, groupId); + + m_failedConstrMemory->resize(m_slvsSystem.faileds); + + return static_cast(m_slvsSystem.result); +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +double SolveSpaceSystem::parameterValue(Slvs_hParam paramId) +{ + return (*m_paramsMemory)[paramId-1].val; +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +std::tuple< std::valarray, + std::valarray, + std::valarray > SolveSpaceSystem::orientationMx(Slvs_hEntity normalIn3dEntityId) +{ + Slvs_Entity e_CS = (*m_entityMemory)[normalIn3dEntityId -1]; + if ( e_CS.type == SLVS_E_NORMAL_IN_3D ) + { + std::valarray quat ={ 0.0, 0.0, 0.0, 0.0 }; + quat[0] = parameterValue(e_CS.param[0]); + quat[1] = parameterValue(e_CS.param[1]); + quat[2] = parameterValue(e_CS.param[2]); + quat[3] = parameterValue(e_CS.param[3]); + std::valarray Ex ={ 0.0,0.0,0.0 }; + std::valarray Ey ={ 0.0,0.0,0.0 }; + std::valarray Ez ={ 0.0,0.0,0.0 }; + + Slvs_QuaternionU(quat[0], quat[1], quat[2], quat[3], + &Ex[0], &Ex[1], &Ex[2]); + Slvs_QuaternionV(quat[0], quat[1], quat[2], quat[3], + &Ey[0], &Ey[1], &Ey[2]); + Slvs_QuaternionN(quat[0], quat[1], quat[2], quat[3], + &Ez[0], &Ez[1], &Ez[2]); + + return std::make_tuple(Ex, Ey, Ez); + } + assert(false); + return std::make_tuple(std::valarray(), std::valarray(), std::valarray()); +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +std::valarray SolveSpaceSystem::global3DPos(Slvs_hEntity pointEntityId) +{ + std::valarray point ={ 0.0,0.0,0.0 }; + + Slvs_Entity pointEntity = (*m_entityMemory)[pointEntityId -1]; + if ( pointEntity.type == SLVS_E_POINT_IN_2D ) + { + std::valarray locPoint ={ 0.0,0.0,0.0 }; + locPoint[0] = parameterValue(pointEntity.param[0]); + locPoint[1] = parameterValue(pointEntity.param[1]); + + Slvs_Entity e_Plane = (*m_entityMemory)[pointEntity.wrkpl - 1]; + std::valarray origin = global3DPos(e_Plane.point[0]); + auto mx = orientationMx(e_Plane.normal); + point = origin + std::get<0>(mx)*locPoint[0] + std::get<1>(mx)*locPoint[1]; + + } + else if ( pointEntity.type == SLVS_E_POINT_IN_3D ) + { + point[0] = parameterValue(pointEntity.param[0]); + point[1] = parameterValue(pointEntity.param[1]); + point[2] = parameterValue(pointEntity.param[2]); + } + + return point; +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- +Slvs_Constraint SolveSpaceSystem::constraint(Slvs_hConstraint constraintId) +{ + return (*m_constraintMemory)[constraintId-1]; +} + +//-------------------------------------------------------------------------------------------------- +/// +//-------------------------------------------------------------------------------------------------- + std::vector SolveSpaceSystem::failedConstraints() const +{ + return (*m_failedConstrMemory); +} diff --git a/ThirdParty/libslvs/constrainteq.cpp b/ThirdParty/libslvs/constrainteq.cpp new file mode 100644 index 0000000000..86ed312f7d --- /dev/null +++ b/ThirdParty/libslvs/constrainteq.cpp @@ -0,0 +1,799 @@ +//----------------------------------------------------------------------------- +// Given a constraint, generate one or more equations in our symbolic algebra +// system to represent that constraint; also various geometric helper +// functions for that. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- +#include "solvespace.h" + +const hConstraint ConstraintBase::NO_CONSTRAINT = { 0 }; + +bool ConstraintBase::HasLabel() const { + switch(type) { + case Type::PT_LINE_DISTANCE: + case Type::PT_PLANE_DISTANCE: + case Type::PT_FACE_DISTANCE: + case Type::PT_PT_DISTANCE: + case Type::PROJ_PT_DISTANCE: + case Type::DIAMETER: + case Type::LENGTH_RATIO: + case Type::LENGTH_DIFFERENCE: + case Type::ANGLE: + case Type::COMMENT: + return true; + + default: + return false; + } +} + +ExprVector ConstraintBase::VectorsParallel3d(ExprVector a, ExprVector b, hParam p) { + return a.Minus(b.ScaledBy(Expr::From(p))); +} + +Expr *ConstraintBase::PointLineDistance(hEntity wrkpl, hEntity hpt, hEntity hln) +{ + EntityBase *ln = SK.GetEntity(hln); + EntityBase *a = SK.GetEntity(ln->point[0]); + EntityBase *b = SK.GetEntity(ln->point[1]); + + EntityBase *p = SK.GetEntity(hpt); + + if(wrkpl.v == EntityBase::FREE_IN_3D.v) { + ExprVector ep = p->PointGetExprs(); + + ExprVector ea = a->PointGetExprs(); + ExprVector eb = b->PointGetExprs(); + ExprVector eab = ea.Minus(eb); + Expr *m = eab.Magnitude(); + + return ((eab.Cross(ea.Minus(ep))).Magnitude())->Div(m); + } else { + Expr *ua, *va, *ub, *vb; + a->PointGetExprsInWorkplane(wrkpl, &ua, &va); + b->PointGetExprsInWorkplane(wrkpl, &ub, &vb); + + Expr *du = ua->Minus(ub); + Expr *dv = va->Minus(vb); + + Expr *u, *v; + p->PointGetExprsInWorkplane(wrkpl, &u, &v); + + Expr *m = ((du->Square())->Plus(dv->Square()))->Sqrt(); + + Expr *proj = (dv->Times(ua->Minus(u)))->Minus( + (du->Times(va->Minus(v)))); + + return proj->Div(m); + } +} + +Expr *ConstraintBase::PointPlaneDistance(ExprVector p, hEntity hpl) { + ExprVector n; + Expr *d; + SK.GetEntity(hpl)->WorkplaneGetPlaneExprs(&n, &d); + return (p.Dot(n))->Minus(d); +} + +Expr *ConstraintBase::Distance(hEntity wrkpl, hEntity hpa, hEntity hpb) { + EntityBase *pa = SK.GetEntity(hpa); + EntityBase *pb = SK.GetEntity(hpb); + ssassert(pa->IsPoint() && pb->IsPoint(), + "Expected two points to measure projected distance between"); + + if(wrkpl.v == EntityBase::FREE_IN_3D.v) { + // This is true distance + ExprVector ea, eb, eab; + ea = pa->PointGetExprs(); + eb = pb->PointGetExprs(); + eab = ea.Minus(eb); + + return eab.Magnitude(); + } else { + // This is projected distance, in the given workplane. + Expr *au, *av, *bu, *bv; + + pa->PointGetExprsInWorkplane(wrkpl, &au, &av); + pb->PointGetExprsInWorkplane(wrkpl, &bu, &bv); + + Expr *du = au->Minus(bu); + Expr *dv = av->Minus(bv); + + return ((du->Square())->Plus(dv->Square()))->Sqrt(); + } +} + +//----------------------------------------------------------------------------- +// Return the cosine of the angle between two vectors. If a workplane is +// specified, then it's the cosine of their projections into that workplane. +//----------------------------------------------------------------------------- +Expr *ConstraintBase::DirectionCosine(hEntity wrkpl, + ExprVector ae, ExprVector be) +{ + if(wrkpl.v == EntityBase::FREE_IN_3D.v) { + Expr *mags = (ae.Magnitude())->Times(be.Magnitude()); + return (ae.Dot(be))->Div(mags); + } else { + EntityBase *w = SK.GetEntity(wrkpl); + ExprVector u = w->Normal()->NormalExprsU(); + ExprVector v = w->Normal()->NormalExprsV(); + Expr *ua = u.Dot(ae); + Expr *va = v.Dot(ae); + Expr *ub = u.Dot(be); + Expr *vb = v.Dot(be); + Expr *maga = (ua->Square()->Plus(va->Square()))->Sqrt(); + Expr *magb = (ub->Square()->Plus(vb->Square()))->Sqrt(); + Expr *dot = (ua->Times(ub))->Plus(va->Times(vb)); + return dot->Div(maga->Times(magb)); + } +} + +ExprVector ConstraintBase::PointInThreeSpace(hEntity workplane, + Expr *u, Expr *v) +{ + EntityBase *w = SK.GetEntity(workplane); + + ExprVector ub = w->Normal()->NormalExprsU(); + ExprVector vb = w->Normal()->NormalExprsV(); + ExprVector ob = w->WorkplaneGetOffsetExprs(); + + return (ub.ScaledBy(u)).Plus(vb.ScaledBy(v)).Plus(ob); +} + +void ConstraintBase::ModifyToSatisfy() { + if(type == Type::ANGLE) { + Vector a = SK.GetEntity(entityA)->VectorGetNum(); + Vector b = SK.GetEntity(entityB)->VectorGetNum(); + if(other) a = a.ScaledBy(-1); + if(workplane.v != EntityBase::FREE_IN_3D.v) { + a = a.ProjectVectorInto(workplane); + b = b.ProjectVectorInto(workplane); + } + double c = (a.Dot(b))/(a.Magnitude() * b.Magnitude()); + valA = acos(c)*180/PI; + } else if(type == Type::PT_ON_LINE) { + EntityBase *eln = SK.GetEntity(entityA); + EntityBase *ea = SK.GetEntity(eln->point[0]); + EntityBase *eb = SK.GetEntity(eln->point[1]); + EntityBase *ep = SK.GetEntity(ptA); + ExprVector exp = ep->PointGetExprsInWorkplane(workplane); + ExprVector exa = ea->PointGetExprsInWorkplane(workplane); + ExprVector exb = eb->PointGetExprsInWorkplane(workplane); + ExprVector exba = exb.Minus(exa); + SK.GetParam(valP)->val = exba.Dot(exp.Minus(exa))->Eval() / exba.Dot(exba)->Eval(); + } else { + // We'll fix these ones up by looking at their symbolic equation; + // that means no extra work. + IdList l = {}; + // Generate the equations even if this is a reference dimension + GenerateEquations(&l, /*forReference=*/true); + ssassert(l.n == 1, "Expected constraint to generate a single equation"); + + // These equations are written in the form f(...) - d = 0, where + // d is the value of the valA. + valA += (l.elem[0].e)->Eval(); + + l.Clear(); + } +} + +void ConstraintBase::AddEq(IdList *l, Expr *expr, int index) const +{ + Equation eq; + eq.e = expr; + eq.h = h.equation(index); + l->Add(&eq); +} + +void ConstraintBase::AddEq(IdList *l, const ExprVector &v, + int baseIndex) const { + AddEq(l, v.x, baseIndex); + AddEq(l, v.y, baseIndex + 1); + if(workplane.v == EntityBase::FREE_IN_3D.v) { + AddEq(l, v.z, baseIndex + 2); + } +} + +void ConstraintBase::Generate(IdList *l) { + switch(type) { + case Type::PARALLEL: + case Type::CUBIC_LINE_TANGENT: + // Add new parameter only when we operate in 3d space + if(workplane.v != EntityBase::FREE_IN_3D.v) break; + // fallthrough + case Type::SAME_ORIENTATION: + case Type::PT_ON_LINE: { + Param p = {}; + valP = h.param(0); + p.h = valP; + l->Add(&p); + break; + } + + default: + break; + } +} + +void ConstraintBase::GenerateEquations(IdList *l, + bool forReference) const { + if(reference && !forReference) return; + + Expr *exA = Expr::From(valA); + switch(type) { + case Type::PT_PT_DISTANCE: + AddEq(l, Distance(workplane, ptA, ptB)->Minus(exA), 0); + return; + + case Type::PROJ_PT_DISTANCE: { + ExprVector pA = SK.GetEntity(ptA)->PointGetExprs(), + pB = SK.GetEntity(ptB)->PointGetExprs(), + dp = pB.Minus(pA); + + ExprVector pp = SK.GetEntity(entityA)->VectorGetExprs(); + pp = pp.WithMagnitude(Expr::From(1.0)); + + AddEq(l, (dp.Dot(pp))->Minus(exA), 0); + return; + } + + case Type::PT_LINE_DISTANCE: + AddEq(l, + PointLineDistance(workplane, ptA, entityA)->Minus(exA), 0); + return; + + case Type::PT_PLANE_DISTANCE: { + ExprVector pt = SK.GetEntity(ptA)->PointGetExprs(); + AddEq(l, (PointPlaneDistance(pt, entityA))->Minus(exA), 0); + return; + } + + case Type::PT_FACE_DISTANCE: { + ExprVector pt = SK.GetEntity(ptA)->PointGetExprs(); + EntityBase *f = SK.GetEntity(entityA); + ExprVector p0 = f->FaceGetPointExprs(); + ExprVector n = f->FaceGetNormalExprs(); + AddEq(l, (pt.Minus(p0)).Dot(n)->Minus(exA), 0); + return; + } + + case Type::EQUAL_LENGTH_LINES: { + EntityBase *a = SK.GetEntity(entityA); + EntityBase *b = SK.GetEntity(entityB); + AddEq(l, Distance(workplane, a->point[0], a->point[1])->Minus( + Distance(workplane, b->point[0], b->point[1])), 0); + return; + } + + // These work on distance squared, since the pt-line distances are + // signed, and we want the absolute value. + case Type::EQ_LEN_PT_LINE_D: { + EntityBase *forLen = SK.GetEntity(entityA); + Expr *d1 = Distance(workplane, forLen->point[0], forLen->point[1]); + Expr *d2 = PointLineDistance(workplane, ptA, entityB); + AddEq(l, (d1->Square())->Minus(d2->Square()), 0); + return; + } + case Type::EQ_PT_LN_DISTANCES: { + Expr *d1 = PointLineDistance(workplane, ptA, entityA); + Expr *d2 = PointLineDistance(workplane, ptB, entityB); + AddEq(l, (d1->Square())->Minus(d2->Square()), 0); + return; + } + + case Type::LENGTH_RATIO: { + EntityBase *a = SK.GetEntity(entityA); + EntityBase *b = SK.GetEntity(entityB); + Expr *la = Distance(workplane, a->point[0], a->point[1]); + Expr *lb = Distance(workplane, b->point[0], b->point[1]); + AddEq(l, (la->Div(lb))->Minus(exA), 0); + return; + } + + case Type::LENGTH_DIFFERENCE: { + EntityBase *a = SK.GetEntity(entityA); + EntityBase *b = SK.GetEntity(entityB); + Expr *la = Distance(workplane, a->point[0], a->point[1]); + Expr *lb = Distance(workplane, b->point[0], b->point[1]); + AddEq(l, (la->Minus(lb))->Minus(exA), 0); + return; + } + + case Type::DIAMETER: { + EntityBase *circle = SK.GetEntity(entityA); + Expr *r = circle->CircleGetRadiusExpr(); + AddEq(l, (r->Times(Expr::From(2)))->Minus(exA), 0); + return; + } + + case Type::EQUAL_RADIUS: { + EntityBase *c1 = SK.GetEntity(entityA); + EntityBase *c2 = SK.GetEntity(entityB); + AddEq(l, (c1->CircleGetRadiusExpr())->Minus( + c2->CircleGetRadiusExpr()), 0); + return; + } + + case Type::EQUAL_LINE_ARC_LEN: { + EntityBase *line = SK.GetEntity(entityA), + *arc = SK.GetEntity(entityB); + + // Get the line length + ExprVector l0 = SK.GetEntity(line->point[0])->PointGetExprs(), + l1 = SK.GetEntity(line->point[1])->PointGetExprs(); + Expr *ll = (l1.Minus(l0)).Magnitude(); + + // And get the arc radius, and the cosine of its angle + EntityBase *ao = SK.GetEntity(arc->point[0]), + *as = SK.GetEntity(arc->point[1]), + *af = SK.GetEntity(arc->point[2]); + + ExprVector aos = (as->PointGetExprs()).Minus(ao->PointGetExprs()), + aof = (af->PointGetExprs()).Minus(ao->PointGetExprs()); + Expr *r = aof.Magnitude(); + + ExprVector n = arc->Normal()->NormalExprsN(); + ExprVector u = aos.WithMagnitude(Expr::From(1.0)); + ExprVector v = n.Cross(u); + // so in our new csys, we start at (1, 0, 0) + Expr *costheta = aof.Dot(u)->Div(r); + Expr *sintheta = aof.Dot(v)->Div(r); + + double thetas, thetaf, dtheta; + arc->ArcGetAngles(&thetas, &thetaf, &dtheta); + Expr *theta; + if(dtheta < 3*PI/4) { + theta = costheta->ACos(); + } else if(dtheta < 5*PI/4) { + // As the angle crosses pi, cos theta is not invertible; + // so use the sine to stop blowing up + theta = Expr::From(PI)->Minus(sintheta->ASin()); + } else { + theta = (Expr::From(2*PI))->Minus(costheta->ACos()); + } + + // And write the equation; r*theta = L + AddEq(l, (r->Times(theta))->Minus(ll), 0); + return; + } + + case Type::POINTS_COINCIDENT: { + EntityBase *a = SK.GetEntity(ptA); + EntityBase *b = SK.GetEntity(ptB); + if(workplane.v == EntityBase::FREE_IN_3D.v) { + ExprVector pa = a->PointGetExprs(); + ExprVector pb = b->PointGetExprs(); + AddEq(l, pa.x->Minus(pb.x), 0); + AddEq(l, pa.y->Minus(pb.y), 1); + AddEq(l, pa.z->Minus(pb.z), 2); + } else { + Expr *au, *av; + Expr *bu, *bv; + a->PointGetExprsInWorkplane(workplane, &au, &av); + b->PointGetExprsInWorkplane(workplane, &bu, &bv); + AddEq(l, au->Minus(bu), 0); + AddEq(l, av->Minus(bv), 1); + } + return; + } + + case Type::PT_IN_PLANE: + // This one works the same, whether projected or not. + AddEq(l, PointPlaneDistance( + SK.GetEntity(ptA)->PointGetExprs(), entityA), 0); + return; + + case Type::PT_ON_FACE: { + // a plane, n dot (p - p0) = 0 + ExprVector p = SK.GetEntity(ptA)->PointGetExprs(); + EntityBase *f = SK.GetEntity(entityA); + ExprVector p0 = f->FaceGetPointExprs(); + ExprVector n = f->FaceGetNormalExprs(); + AddEq(l, (p.Minus(p0)).Dot(n), 0); + return; + } + + case Type::PT_ON_LINE: { + EntityBase *ln = SK.GetEntity(entityA); + EntityBase *a = SK.GetEntity(ln->point[0]); + EntityBase *b = SK.GetEntity(ln->point[1]); + EntityBase *p = SK.GetEntity(ptA); + + ExprVector ep = p->PointGetExprsInWorkplane(workplane); + ExprVector ea = a->PointGetExprsInWorkplane(workplane); + ExprVector eb = b->PointGetExprsInWorkplane(workplane); + + ExprVector ptOnLine = ea.Plus(eb.Minus(ea).ScaledBy(Expr::From(valP))); + ExprVector eq = ptOnLine.Minus(ep); + + AddEq(l, eq); + return; + } + + case Type::PT_ON_CIRCLE: { + // This actually constrains the point to lie on the cylinder. + EntityBase *circle = SK.GetEntity(entityA); + ExprVector center = SK.GetEntity(circle->point[0])->PointGetExprs(); + ExprVector pt = SK.GetEntity(ptA)->PointGetExprs(); + EntityBase *normal = SK.GetEntity(circle->normal); + ExprVector u = normal->NormalExprsU(), + v = normal->NormalExprsV(); + + Expr *du = (center.Minus(pt)).Dot(u), + *dv = (center.Minus(pt)).Dot(v); + + Expr *r = circle->CircleGetRadiusExpr(); + + AddEq(l, du->Square()->Plus(dv->Square())->Sqrt()->Minus(r), 0); + return; + } + + case Type::AT_MIDPOINT: + if(workplane.v == EntityBase::FREE_IN_3D.v) { + EntityBase *ln = SK.GetEntity(entityA); + ExprVector a = SK.GetEntity(ln->point[0])->PointGetExprs(); + ExprVector b = SK.GetEntity(ln->point[1])->PointGetExprs(); + ExprVector m = (a.Plus(b)).ScaledBy(Expr::From(0.5)); + + if(ptA.v) { + ExprVector p = SK.GetEntity(ptA)->PointGetExprs(); + AddEq(l, (m.x)->Minus(p.x), 0); + AddEq(l, (m.y)->Minus(p.y), 1); + AddEq(l, (m.z)->Minus(p.z), 2); + } else { + AddEq(l, PointPlaneDistance(m, entityB), 0); + } + } else { + EntityBase *ln = SK.GetEntity(entityA); + EntityBase *a = SK.GetEntity(ln->point[0]); + EntityBase *b = SK.GetEntity(ln->point[1]); + + Expr *au, *av, *bu, *bv; + a->PointGetExprsInWorkplane(workplane, &au, &av); + b->PointGetExprsInWorkplane(workplane, &bu, &bv); + Expr *mu = Expr::From(0.5)->Times(au->Plus(bu)); + Expr *mv = Expr::From(0.5)->Times(av->Plus(bv)); + + if(ptA.v) { + EntityBase *p = SK.GetEntity(ptA); + Expr *pu, *pv; + p->PointGetExprsInWorkplane(workplane, &pu, &pv); + AddEq(l, pu->Minus(mu), 0); + AddEq(l, pv->Minus(mv), 1); + } else { + ExprVector m = PointInThreeSpace(workplane, mu, mv); + AddEq(l, PointPlaneDistance(m, entityB), 0); + } + } + return; + + case Type::SYMMETRIC: + if(workplane.v == EntityBase::FREE_IN_3D.v) { + EntityBase *plane = SK.GetEntity(entityA); + EntityBase *ea = SK.GetEntity(ptA); + EntityBase *eb = SK.GetEntity(ptB); + ExprVector a = ea->PointGetExprs(); + ExprVector b = eb->PointGetExprs(); + + // The midpoint of the line connecting the symmetric points + // lies on the plane of the symmetry. + ExprVector m = (a.Plus(b)).ScaledBy(Expr::From(0.5)); + AddEq(l, PointPlaneDistance(m, plane->h), 0); + + // And projected into the plane of symmetry, the points are + // coincident. + Expr *au, *av, *bu, *bv; + ea->PointGetExprsInWorkplane(plane->h, &au, &av); + eb->PointGetExprsInWorkplane(plane->h, &bu, &bv); + AddEq(l, au->Minus(bu), 1); + AddEq(l, av->Minus(bv), 2); + } else { + EntityBase *plane = SK.GetEntity(entityA); + EntityBase *a = SK.GetEntity(ptA); + EntityBase *b = SK.GetEntity(ptB); + + Expr *au, *av, *bu, *bv; + a->PointGetExprsInWorkplane(workplane, &au, &av); + b->PointGetExprsInWorkplane(workplane, &bu, &bv); + Expr *mu = Expr::From(0.5)->Times(au->Plus(bu)); + Expr *mv = Expr::From(0.5)->Times(av->Plus(bv)); + + ExprVector m = PointInThreeSpace(workplane, mu, mv); + AddEq(l, PointPlaneDistance(m, plane->h), 0); + + // Construct a vector within the workplane that is normal + // to the symmetry pane's normal (i.e., that lies in the + // plane of symmetry). The line connecting the points is + // perpendicular to that constructed vector. + EntityBase *w = SK.GetEntity(workplane); + ExprVector u = w->Normal()->NormalExprsU(); + ExprVector v = w->Normal()->NormalExprsV(); + + ExprVector pa = a->PointGetExprs(); + ExprVector pb = b->PointGetExprs(); + ExprVector n; + Expr *d; + plane->WorkplaneGetPlaneExprs(&n, &d); + AddEq(l, (n.Cross(u.Cross(v))).Dot(pa.Minus(pb)), 1); + } + return; + + case Type::SYMMETRIC_HORIZ: + case Type::SYMMETRIC_VERT: { + ssassert(workplane.v != Entity::FREE_IN_3D.v, + "Unexpected horizontal/vertical symmetric constraint in 3d"); + + EntityBase *a = SK.GetEntity(ptA); + EntityBase *b = SK.GetEntity(ptB); + + Expr *au, *av, *bu, *bv; + a->PointGetExprsInWorkplane(workplane, &au, &av); + b->PointGetExprsInWorkplane(workplane, &bu, &bv); + + if(type == Type::SYMMETRIC_HORIZ) { + AddEq(l, av->Minus(bv), 0); + AddEq(l, au->Plus(bu), 1); + } else { + AddEq(l, au->Minus(bu), 0); + AddEq(l, av->Plus(bv), 1); + } + return; + } + + case Type::SYMMETRIC_LINE: { + EntityBase *pa = SK.GetEntity(ptA); + EntityBase *pb = SK.GetEntity(ptB); + + Expr *pau, *pav, *pbu, *pbv; + pa->PointGetExprsInWorkplane(workplane, &pau, &pav); + pb->PointGetExprsInWorkplane(workplane, &pbu, &pbv); + + EntityBase *ln = SK.GetEntity(entityA); + EntityBase *la = SK.GetEntity(ln->point[0]); + EntityBase *lb = SK.GetEntity(ln->point[1]); + Expr *lau, *lav, *lbu, *lbv; + la->PointGetExprsInWorkplane(workplane, &lau, &lav); + lb->PointGetExprsInWorkplane(workplane, &lbu, &lbv); + + Expr *dpu = pbu->Minus(pau), *dpv = pbv->Minus(pav); + Expr *dlu = lbu->Minus(lau), *dlv = lbv->Minus(lav); + + // The line through the points is perpendicular to the line + // of symmetry. + AddEq(l, (dlu->Times(dpu))->Plus(dlv->Times(dpv)), 0); + + // And the signed distances of the points to the line are + // equal in magnitude and opposite in sign, so sum to zero + Expr *dista = (dlv->Times(lau->Minus(pau)))->Minus( + (dlu->Times(lav->Minus(pav)))); + Expr *distb = (dlv->Times(lau->Minus(pbu)))->Minus( + (dlu->Times(lav->Minus(pbv)))); + AddEq(l, dista->Plus(distb), 1); + + return; + } + + case Type::HORIZONTAL: + case Type::VERTICAL: { + ssassert(workplane.v != Entity::FREE_IN_3D.v, + "Unexpected horizontal/vertical constraint in 3d"); + + hEntity ha, hb; + if(entityA.v) { + EntityBase *e = SK.GetEntity(entityA); + ha = e->point[0]; + hb = e->point[1]; + } else { + ha = ptA; + hb = ptB; + } + EntityBase *a = SK.GetEntity(ha); + EntityBase *b = SK.GetEntity(hb); + + Expr *au, *av, *bu, *bv; + a->PointGetExprsInWorkplane(workplane, &au, &av); + b->PointGetExprsInWorkplane(workplane, &bu, &bv); + + AddEq(l, (type == Type::HORIZONTAL) ? av->Minus(bv) : au->Minus(bu), 0); + return; + } + + case Type::SAME_ORIENTATION: { + EntityBase *a = SK.GetEntity(entityA); + EntityBase *b = SK.GetEntity(entityB); + + ExprVector au = a->NormalExprsU(), + an = a->NormalExprsN(); + ExprVector bu = b->NormalExprsU(), + bv = b->NormalExprsV(), + bn = b->NormalExprsN(); + + ExprVector eq = VectorsParallel3d(an, bn, valP); + AddEq(l, eq.x, 0); + AddEq(l, eq.y, 1); + AddEq(l, eq.z, 2); + Expr *d1 = au.Dot(bv); + Expr *d2 = au.Dot(bu); + // Allow either orientation for the coordinate system, depending + // on how it was drawn. + if(fabs(d1->Eval()) < fabs(d2->Eval())) { + AddEq(l, d1, 3); + } else { + AddEq(l, d2, 3); + } + return; + } + + case Type::PERPENDICULAR: + case Type::ANGLE: { + EntityBase *a = SK.GetEntity(entityA); + EntityBase *b = SK.GetEntity(entityB); + ExprVector ae = a->VectorGetExprs(); + ExprVector be = b->VectorGetExprs(); + if(other) ae = ae.ScaledBy(Expr::From(-1)); + Expr *c = DirectionCosine(workplane, ae, be); + + if(type == Type::ANGLE) { + // The direction cosine is equal to the cosine of the + // specified angle + Expr *rads = exA->Times(Expr::From(PI/180)), + *rc = rads->Cos(); + double arc = fabs(rc->Eval()); + // avoid false detection of inconsistent systems by gaining + // up as the difference in dot products gets small at small + // angles; doubles still have plenty of precision, only + // problem is that rank test + Expr *mult = Expr::From(arc > 0.99 ? 0.01/(1.00001 - arc) : 1); + AddEq(l, (c->Minus(rc))->Times(mult), 0); + } else { + // The dot product (and therefore the direction cosine) + // is equal to zero, perpendicular. + AddEq(l, c, 0); + } + return; + } + + case Type::EQUAL_ANGLE: { + EntityBase *a = SK.GetEntity(entityA); + EntityBase *b = SK.GetEntity(entityB); + EntityBase *c = SK.GetEntity(entityC); + EntityBase *d = SK.GetEntity(entityD); + ExprVector ae = a->VectorGetExprs(); + ExprVector be = b->VectorGetExprs(); + ExprVector ce = c->VectorGetExprs(); + ExprVector de = d->VectorGetExprs(); + + if(other) ae = ae.ScaledBy(Expr::From(-1)); + + Expr *cab = DirectionCosine(workplane, ae, be); + Expr *ccd = DirectionCosine(workplane, ce, de); + + AddEq(l, cab->Minus(ccd), 0); + return; + } + + case Type::ARC_LINE_TANGENT: { + EntityBase *arc = SK.GetEntity(entityA); + EntityBase *line = SK.GetEntity(entityB); + + ExprVector ac = SK.GetEntity(arc->point[0])->PointGetExprs(); + ExprVector ap = + SK.GetEntity(arc->point[other ? 2 : 1])->PointGetExprs(); + + ExprVector ld = line->VectorGetExprs(); + + // The line is perpendicular to the radius + AddEq(l, ld.Dot(ac.Minus(ap)), 0); + return; + } + + case Type::CUBIC_LINE_TANGENT: { + EntityBase *cubic = SK.GetEntity(entityA); + EntityBase *line = SK.GetEntity(entityB); + + ExprVector a; + if(other) { + a = cubic->CubicGetFinishTangentExprs(); + } else { + a = cubic->CubicGetStartTangentExprs(); + } + + ExprVector b = line->VectorGetExprs(); + + if(workplane.v == EntityBase::FREE_IN_3D.v) { + ExprVector eq = VectorsParallel3d(a, b, valP); + AddEq(l, eq); + } else { + EntityBase *w = SK.GetEntity(workplane); + ExprVector wn = w->Normal()->NormalExprsN(); + AddEq(l, (a.Cross(b)).Dot(wn), 0); + } + return; + } + + case Type::CURVE_CURVE_TANGENT: { + bool parallel = true; + int i; + ExprVector dir[2]; + for(i = 0; i < 2; i++) { + EntityBase *e = SK.GetEntity((i == 0) ? entityA : entityB); + bool oth = (i == 0) ? other : other2; + + if(e->type == Entity::Type::ARC_OF_CIRCLE) { + ExprVector center, endpoint; + center = SK.GetEntity(e->point[0])->PointGetExprs(); + endpoint = + SK.GetEntity(e->point[oth ? 2 : 1])->PointGetExprs(); + dir[i] = endpoint.Minus(center); + // We're using the vector from the center of the arc to + // an endpoint; so that's normal to the tangent, not + // parallel. + parallel = !parallel; + } else if(e->type == Entity::Type::CUBIC) { // BRANCH_ALWAYS_TAKEN + if(oth) { + dir[i] = e->CubicGetFinishTangentExprs(); + } else { + dir[i] = e->CubicGetStartTangentExprs(); + } + } else { + ssassert(false, "Unexpected entity types for CURVE_CURVE_TANGENT"); + } + } + if(parallel) { + EntityBase *w = SK.GetEntity(workplane); + ExprVector wn = w->Normal()->NormalExprsN(); + AddEq(l, ((dir[0]).Cross(dir[1])).Dot(wn), 0); + } else { + AddEq(l, (dir[0]).Dot(dir[1]), 0); + } + return; + } + + case Type::PARALLEL: { + EntityBase *ea = SK.GetEntity(entityA), *eb = SK.GetEntity(entityB); + ExprVector a = ea->VectorGetExprsInWorkplane(workplane); + ExprVector b = eb->VectorGetExprsInWorkplane(workplane); + + if(workplane.v == EntityBase::FREE_IN_3D.v) { + ExprVector eq = VectorsParallel3d(a, b, valP); + AddEq(l, eq); + } else { + // We use expressions written in workplane csys, so we can assume the workplane + // normal is (0, 0, 1). We can write the equation as: + // Expr *eq = a.Cross(b).Dot(ExprVector::From(0.0, 0.0, 1.0)); + // but this will just result in elimination of x and y terms after dot product. + // We can only use the z expression: + // Expr *eq = a.Cross(b).z; + // but it's more efficient to write it in the terms of pseudo-scalar product: + Expr *eq = (a.x->Times(b.y))->Minus(a.y->Times(b.x)); + AddEq(l, eq, 0); + } + + return; + } + + case Type::WHERE_DRAGGED: { + EntityBase *ep = SK.GetEntity(ptA); + if(workplane.v == EntityBase::FREE_IN_3D.v) { + ExprVector ev = ep->PointGetExprs(); + Vector v = ep->PointGetNum(); + + AddEq(l, ev.x->Minus(Expr::From(v.x)), 0); + AddEq(l, ev.y->Minus(Expr::From(v.y)), 1); + AddEq(l, ev.z->Minus(Expr::From(v.z)), 2); + } else { + Expr *u, *v; + ep->PointGetExprsInWorkplane(workplane, &u, &v); + AddEq(l, u->Minus(Expr::From(u->Eval())), 0); + AddEq(l, v->Minus(Expr::From(v->Eval())), 1); + } + return; + } + + case Type::COMMENT: + return; + } + ssassert(false, "Unexpected constraint ID"); +} + diff --git a/ThirdParty/libslvs/dsc.h b/ThirdParty/libslvs/dsc.h new file mode 100644 index 0000000000..bd5c7f35da --- /dev/null +++ b/ThirdParty/libslvs/dsc.h @@ -0,0 +1,578 @@ +//----------------------------------------------------------------------------- +// Data structures used frequently in the program, various kinds of vectors +// (of real numbers, not symbolic algebra stuff) and our templated lists. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- +#ifndef __DSC_H +#define __DSC_H + +#include "solvespace.h" + +class Vector; +class Vector4; +class Point2d; +class hEntity; +class hParam; + +class Quaternion { +public: + // a + (vx)*i + (vy)*j + (vz)*k + double w, vx, vy, vz; + + static const Quaternion IDENTITY; + + static Quaternion From(double w, double vx, double vy, double vz); + static Quaternion From(hParam w, hParam vx, hParam vy, hParam vz); + static Quaternion From(Vector u, Vector v); + static Quaternion From(Vector axis, double dtheta); + + Quaternion Plus(Quaternion b) const; + Quaternion Minus(Quaternion b) const; + Quaternion ScaledBy(double s) const; + double Magnitude() const; + Quaternion WithMagnitude(double s) const; + + // Call a rotation matrix [ u' v' n' ]'; this returns the first and + // second rows, where that matrix is generated by this quaternion + Vector RotationU() const; + Vector RotationV() const; + Vector RotationN() const; + Vector Rotate(Vector p) const; + + Quaternion ToThe(double p) const; + Quaternion Inverse() const; + Quaternion Times(Quaternion b) const; + Quaternion Mirror() const; +}; + +class Vector { +public: + double x, y, z; + + static Vector From(double x, double y, double z); + static Vector From(hParam x, hParam y, hParam z); + static Vector AtIntersectionOfPlanes(Vector n1, double d1, + Vector n2, double d2); + static Vector AtIntersectionOfLines(Vector a0, Vector a1, + Vector b0, Vector b1, + bool *skew, + double *pa=NULL, double *pb=NULL); + static Vector AtIntersectionOfPlaneAndLine(Vector n, double d, + Vector p0, Vector p1, + bool *parallel); + static Vector AtIntersectionOfPlanes(Vector na, double da, + Vector nb, double db, + Vector nc, double dc, bool *parallel); + static void ClosestPointBetweenLines(Vector pa, Vector da, + Vector pb, Vector db, + double *ta, double *tb); + + double Element(int i) const; + bool Equals(Vector v, double tol=LENGTH_EPS) const; + bool EqualsExactly(Vector v) const; + Vector Plus(Vector b) const; + Vector Minus(Vector b) const; + Vector Negated() const; + Vector Cross(Vector b) const; + double DirectionCosineWith(Vector b) const; + double Dot(Vector b) const; + Vector Normal(int which) const; + Vector RotatedAbout(Vector orig, Vector axis, double theta) const; + Vector RotatedAbout(Vector axis, double theta) const; + Vector DotInToCsys(Vector u, Vector v, Vector n) const; + Vector ScaleOutOfCsys(Vector u, Vector v, Vector n) const; + double DistanceToLine(Vector p0, Vector dp) const; + double DistanceToPlane(Vector normal, Vector origin) const; + bool OnLineSegment(Vector a, Vector b, double tol=LENGTH_EPS) const; + Vector ClosestPointOnLine(Vector p0, Vector deltal) const; + double Magnitude() const; + double MagSquared() const; + Vector WithMagnitude(double s) const; + Vector ScaledBy(double s) const; + Vector ProjectInto(hEntity wrkpl) const; + Vector ProjectVectorInto(hEntity wrkpl) const; + double DivPivoting(Vector delta) const; + Vector ClosestOrtho() const; + void MakeMaxMin(Vector *maxv, Vector *minv) const; + Vector ClampWithin(double minv, double maxv) const; + static bool BoundingBoxesDisjoint(Vector amax, Vector amin, + Vector bmax, Vector bmin); + static bool BoundingBoxIntersectsLine(Vector amax, Vector amin, + Vector p0, Vector p1, bool asSegment); + bool OutsideAndNotOn(Vector maxv, Vector minv) const; + Vector InPerspective(Vector u, Vector v, Vector n, + Vector origin, double cameraTan) const; + Point2d Project2d(Vector u, Vector v) const; + Point2d ProjectXy() const; + Vector4 Project4d() const; +}; + +struct VectorHash { + size_t operator()(const Vector &v) const; +}; + +struct VectorPred { + bool operator()(Vector a, Vector b) const; +}; + +class Vector4 { +public: + double w, x, y, z; + + static Vector4 From(double w, double x, double y, double z); + static Vector4 From(double w, Vector v3); + static Vector4 Blend(Vector4 a, Vector4 b, double t); + + Vector4 Plus(Vector4 b) const; + Vector4 Minus(Vector4 b) const; + Vector4 ScaledBy(double s) const; + Vector PerspectiveProject() const; +}; + +class Point2d { +public: + double x, y; + + static Point2d From(double x, double y); + static Point2d FromPolar(double r, double a); + + Point2d Plus(const Point2d &b) const; + Point2d Minus(const Point2d &b) const; + Point2d ScaledBy(double s) const; + double DivPivoting(Point2d delta) const; + double Dot(Point2d p) const; + double DistanceTo(const Point2d &p) const; + double DistanceToLine(const Point2d &p0, const Point2d &dp, bool asSegment) const; + double DistanceToLineSigned(const Point2d &p0, const Point2d &dp, bool asSegment) const; + double Angle() const; + double AngleTo(const Point2d &p) const; + double Magnitude() const; + double MagSquared() const; + Point2d WithMagnitude(double v) const; + Point2d Normal() const; + bool Equals(Point2d v, double tol=LENGTH_EPS) const; +}; + +// A simple list +template +class List { +public: + T *elem; + int n; + int elemsAllocated; + + void ReserveMore(int howMuch) { + if(n + howMuch > elemsAllocated) { + elemsAllocated = n + howMuch; + T *newElem = (T *)MemAlloc((size_t)elemsAllocated*sizeof(elem[0])); + for(int i = 0; i < n; i++) { + new(&newElem[i]) T(std::move(elem[i])); + elem[i].~T(); + } + MemFree(elem); + elem = newElem; + } + } + + void AllocForOneMore() { + if(n >= elemsAllocated) { + ReserveMore((elemsAllocated + 32)*2 - n); + } + } + + void Add(const T *t) { + AllocForOneMore(); + new(&elem[n++]) T(*t); + } + + void AddToBeginning(const T *t) { + AllocForOneMore(); + new(&elem[n]) T(); + std::move_backward(elem, elem + 1, elem + n + 1); + elem[0] = *t; + n++; + } + + T *First() { + return (n == 0) ? NULL : &(elem[0]); + } + const T *First() const { + return (n == 0) ? NULL : &(elem[0]); + } + T *NextAfter(T *prev) { + if(!prev) return NULL; + if(prev - elem == (n - 1)) return NULL; + return prev + 1; + } + const T *NextAfter(const T *prev) const { + if(!prev) return NULL; + if(prev - elem == (n - 1)) return NULL; + return prev + 1; + } + + T *begin() { return &elem[0]; } + T *end() { return &elem[n]; } + const T *begin() const { return &elem[0]; } + const T *end() const { return &elem[n]; } + + void ClearTags() { + int i; + for(i = 0; i < n; i++) { + elem[i].tag = 0; + } + } + + void Clear() { + for(int i = 0; i < n; i++) + elem[i].~T(); + if(elem) MemFree(elem); + elem = NULL; + n = elemsAllocated = 0; + } + + void RemoveTagged() { + int src, dest; + dest = 0; + for(src = 0; src < n; src++) { + if(elem[src].tag) { + // this item should be deleted + } else { + if(src != dest) { + elem[dest] = elem[src]; + } + dest++; + } + } + for(int i = dest; i < n; i++) + elem[i].~T(); + n = dest; + // and elemsAllocated is untouched, because we didn't resize + } + + void RemoveLast(int cnt) { + ssassert(n >= cnt, "Removing more elements than the list contains"); + for(int i = n - cnt; i < n; i++) + elem[i].~T(); + n -= cnt; + // and elemsAllocated is untouched, same as in RemoveTagged + } + + void Reverse() { + int i; + for(i = 0; i < (n/2); i++) { + swap(elem[i], elem[(n-1)-i]); + } + } +}; + +// A list, where each element has an integer identifier. The list is kept +// sorted by that identifier, and items can be looked up in log n time by +// id. +template +class IdList { +public: + T *elem; + int n; + int elemsAllocated; + + uint32_t MaximumId() { + if(n == 0) { + return 0; + } else { + return elem[n - 1].h.v; + } + } + + H AddAndAssignId(T *t) { + t->h.v = (MaximumId() + 1); + Add(t); + + return t->h; + } + + void ReserveMore(int howMuch) { + if(n + howMuch > elemsAllocated) { + elemsAllocated = n + howMuch; + T *newElem = (T *)MemAlloc((size_t)elemsAllocated*sizeof(elem[0])); + for(int i = 0; i < n; i++) { + new(&newElem[i]) T(std::move(elem[i])); + elem[i].~T(); + } + MemFree(elem); + elem = newElem; + } + } + + void Add(T *t) { + if(n >= elemsAllocated) { + ReserveMore((elemsAllocated + 32)*2 - n); + } + + int first = 0, last = n; + // We know that we must insert within the closed interval [first,last] + while(first != last) { + int mid = (first + last)/2; + H hm = elem[mid].h; + ssassert(hm.v != t->h.v, "Handle isn't unique"); + if(hm.v > t->h.v) { + last = mid; + } else if(hm.v < t->h.v) { + first = mid + 1; + } + } + + int i = first; + new(&elem[n]) T(); + std::move_backward(elem + i, elem + n, elem + n + 1); + elem[i] = *t; + n++; + } + + T *FindById(H h) { + T *t = FindByIdNoOops(h); + ssassert(t != NULL, "Cannot find handle"); + return t; + } + + int IndexOf(H h) { + int first = 0, last = n-1; + while(first <= last) { + int mid = (first + last)/2; + H hm = elem[mid].h; + if(hm.v > h.v) { + last = mid-1; // and first stays the same + } else if(hm.v < h.v) { + first = mid+1; // and last stays the same + } else { + return mid; + } + } + return -1; + } + + T *FindByIdNoOops(H h) { + int first = 0, last = n-1; + while(first <= last) { + int mid = (first + last)/2; + H hm = elem[mid].h; + if(hm.v > h.v) { + last = mid-1; // and first stays the same + } else if(hm.v < h.v) { + first = mid+1; // and last stays the same + } else { + return &(elem[mid]); + } + } + return NULL; + } + + T *First() { + return (n == 0) ? NULL : &(elem[0]); + } + T *NextAfter(T *prev) { + if(!prev) return NULL; + if(prev - elem == (n - 1)) return NULL; + return prev + 1; + } + + T *begin() { return &elem[0]; } + T *end() { return &elem[n]; } + const T *begin() const { return &elem[0]; } + const T *end() const { return &elem[n]; } + + void ClearTags() { + int i; + for(i = 0; i < n; i++) { + elem[i].tag = 0; + } + } + + void Tag(H h, int tag) { + int i; + for(i = 0; i < n; i++) { + if(elem[i].h.v == h.v) { + elem[i].tag = tag; + } + } + } + + void RemoveTagged() { + int src, dest; + dest = 0; + for(src = 0; src < n; src++) { + if(elem[src].tag) { + // this item should be deleted + elem[src].Clear(); + } else { + if(src != dest) { + elem[dest] = elem[src]; + } + dest++; + } + } + for(int i = dest; i < n; i++) + elem[i].~T(); + n = dest; + // and elemsAllocated is untouched, because we didn't resize + } + void RemoveById(H h) { + ClearTags(); + FindById(h)->tag = 1; + RemoveTagged(); + } + + void MoveSelfInto(IdList *l) { + l->Clear(); + *l = *this; + elemsAllocated = n = 0; + elem = NULL; + } + + void DeepCopyInto(IdList *l) { + l->Clear(); + l->elem = (T *)MemAlloc(elemsAllocated * sizeof(elem[0])); + for(int i = 0; i < n; i++) + new(&l->elem[i]) T(elem[i]); + l->elemsAllocated = elemsAllocated; + l->n = n; + } + + void Clear() { + for(int i = 0; i < n; i++) { + elem[i].Clear(); + elem[i].~T(); + } + elemsAllocated = n = 0; + if(elem) MemFree(elem); + elem = NULL; + } + +}; + +class BandedMatrix { +public: + enum { + MAX_UNKNOWNS = 16, + RIGHT_OF_DIAG = 1, + LEFT_OF_DIAG = 2 + }; + + double A[MAX_UNKNOWNS][MAX_UNKNOWNS]; + double B[MAX_UNKNOWNS]; + double X[MAX_UNKNOWNS]; + int n; + + void Solve(); +}; + +#define RGBi(r, g, b) RgbaColor::From((r), (g), (b)) +#define RGBf(r, g, b) RgbaColor::FromFloat((float)(r), (float)(g), (float)(b)) + +// Note: sizeof(class RgbaColor) should be exactly 4 +// +class RgbaColor { +public: + uint8_t red, green, blue, alpha; + + float redF() const { return (float)red / 255.0f; } + float greenF() const { return (float)green / 255.0f; } + float blueF() const { return (float)blue / 255.0f; } + float alphaF() const { return (float)alpha / 255.0f; } + + bool IsEmpty() const { return alpha == 0; } + + bool Equals(RgbaColor c) const { + return + c.red == red && + c.green == green && + c.blue == blue && + c.alpha == alpha; + } + + RgbaColor WithAlpha(uint8_t newAlpha) const { + RgbaColor color = *this; + color.alpha = newAlpha; + return color; + } + + uint32_t ToPackedIntBGRA() const { + return + blue | + (uint32_t)(green << 8) | + (uint32_t)(red << 16) | + (uint32_t)((255 - alpha) << 24); + } + + uint32_t ToPackedInt() const { + return + red | + (uint32_t)(green << 8) | + (uint32_t)(blue << 16) | + (uint32_t)((255 - alpha) << 24); + } + + uint32_t ToARGB32() const { + return + blue | + (uint32_t)(green << 8) | + (uint32_t)(red << 16) | + (uint32_t)(alpha << 24); + } + + static RgbaColor From(int r, int g, int b, int a = 255) { + RgbaColor c; + c.red = (uint8_t)r; + c.green = (uint8_t)g; + c.blue = (uint8_t)b; + c.alpha = (uint8_t)a; + return c; + } + + static RgbaColor FromFloat(float r, float g, float b, float a = 1.0) { + return From( + (int)(255.1f * r), + (int)(255.1f * g), + (int)(255.1f * b), + (int)(255.1f * a)); + } + + static RgbaColor FromPackedInt(uint32_t rgba) { + return From( + (int)((rgba) & 0xff), + (int)((rgba >> 8) & 0xff), + (int)((rgba >> 16) & 0xff), + (int)(255 - ((rgba >> 24) & 0xff))); + } + + static RgbaColor FromPackedIntBGRA(uint32_t bgra) { + return From( + (int)((bgra >> 16) & 0xff), + (int)((bgra >> 8) & 0xff), + (int)((bgra) & 0xff), + (int)(255 - ((bgra >> 24) & 0xff))); + } +}; + +struct RgbaColorCompare { + bool operator()(RgbaColor a, RgbaColor b) const { + return a.ToARGB32() < b.ToARGB32(); + } +}; + +class BBox { +public: + Vector minp; + Vector maxp; + + static BBox From(const Vector &p0, const Vector &p1); + + Vector GetOrigin() const; + Vector GetExtents() const; + + void Include(const Vector &v, double r = 0.0); + bool Overlaps(const BBox &b1) const; + bool Contains(const Point2d &p, double r = 0.0) const; +}; + +#endif diff --git a/ThirdParty/libslvs/entity.cpp b/ThirdParty/libslvs/entity.cpp new file mode 100644 index 0000000000..4f4eabe165 --- /dev/null +++ b/ThirdParty/libslvs/entity.cpp @@ -0,0 +1,875 @@ +//----------------------------------------------------------------------------- +// The implementation of our entities in the symbolic algebra system, methods +// to return a symbolic representation of the entity (line by its endpoints, +// circle by center and radius, etc.). +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- +#include "solvespace.h" + +const hEntity EntityBase::FREE_IN_3D = { 0 }; +const hEntity EntityBase::NO_ENTITY = { 0 }; + +bool EntityBase::HasVector() const { + switch(type) { + case Type::LINE_SEGMENT: + case Type::NORMAL_IN_3D: + case Type::NORMAL_IN_2D: + case Type::NORMAL_N_COPY: + case Type::NORMAL_N_ROT: + case Type::NORMAL_N_ROT_AA: + return true; + + default: + return false; + } +} + +ExprVector EntityBase::VectorGetExprsInWorkplane(hEntity wrkpl) const { + switch(type) { + case Type::LINE_SEGMENT: + return (SK.GetEntity(point[0])->PointGetExprsInWorkplane(wrkpl)).Minus( + SK.GetEntity(point[1])->PointGetExprsInWorkplane(wrkpl)); + + case Type::NORMAL_IN_3D: + case Type::NORMAL_IN_2D: + case Type::NORMAL_N_COPY: + case Type::NORMAL_N_ROT: + case Type::NORMAL_N_ROT_AA: { + ExprVector ev = NormalExprsN(); + if(wrkpl.v == EntityBase::FREE_IN_3D.v) { + return ev; + } + // Get the offset and basis vectors for this weird exotic csys. + EntityBase *w = SK.GetEntity(wrkpl); + ExprVector wu = w->Normal()->NormalExprsU(); + ExprVector wv = w->Normal()->NormalExprsV(); + + // Get our coordinates in three-space, and project them into that + // coordinate system. + ExprVector result; + result.x = ev.Dot(wu); + result.y = ev.Dot(wv); + result.z = Expr::From(0.0); + return result; + } + default: ssassert(false, "Unexpected entity type"); + } +} + +ExprVector EntityBase::VectorGetExprs() const { + return VectorGetExprsInWorkplane(EntityBase::FREE_IN_3D); +} + +Vector EntityBase::VectorGetNum() const { + switch(type) { + case Type::LINE_SEGMENT: + return (SK.GetEntity(point[0])->PointGetNum()).Minus( + SK.GetEntity(point[1])->PointGetNum()); + + case Type::NORMAL_IN_3D: + case Type::NORMAL_IN_2D: + case Type::NORMAL_N_COPY: + case Type::NORMAL_N_ROT: + case Type::NORMAL_N_ROT_AA: + return NormalN(); + + default: ssassert(false, "Unexpected entity type"); + } +} + +Vector EntityBase::VectorGetRefPoint() const { + switch(type) { + case Type::LINE_SEGMENT: + return ((SK.GetEntity(point[0])->PointGetNum()).Plus( + SK.GetEntity(point[1])->PointGetNum())).ScaledBy(0.5); + + case Type::NORMAL_IN_3D: + case Type::NORMAL_IN_2D: + case Type::NORMAL_N_COPY: + case Type::NORMAL_N_ROT: + case Type::NORMAL_N_ROT_AA: + return SK.GetEntity(point[0])->PointGetNum(); + + default: ssassert(false, "Unexpected entity type"); + } +} + +Vector EntityBase::VectorGetStartPoint() const { + switch(type) { + case Type::LINE_SEGMENT: + return SK.GetEntity(point[1])->PointGetNum(); + + case Type::NORMAL_IN_3D: + case Type::NORMAL_IN_2D: + case Type::NORMAL_N_COPY: + case Type::NORMAL_N_ROT: + case Type::NORMAL_N_ROT_AA: + return SK.GetEntity(point[0])->PointGetNum(); + + default: ssassert(false, "Unexpected entity type"); + } +} + +bool EntityBase::IsCircle() const { + return (type == Type::CIRCLE) || (type == Type::ARC_OF_CIRCLE); +} + +Expr *EntityBase::CircleGetRadiusExpr() const { + if(type == Type::CIRCLE) { + return SK.GetEntity(distance)->DistanceGetExpr(); + } else if(type == Type::ARC_OF_CIRCLE) { + return Constraint::Distance(workplane, point[0], point[1]); + } else ssassert(false, "Unexpected entity type"); +} + +double EntityBase::CircleGetRadiusNum() const { + if(type == Type::CIRCLE) { + return SK.GetEntity(distance)->DistanceGetNum(); + } else if(type == Type::ARC_OF_CIRCLE) { + Vector c = SK.GetEntity(point[0])->PointGetNum(); + Vector pa = SK.GetEntity(point[1])->PointGetNum(); + return (pa.Minus(c)).Magnitude(); + } else ssassert(false, "Unexpected entity type"); +} + +void EntityBase::ArcGetAngles(double *thetaa, double *thetab, double *dtheta) const { + ssassert(type == Type::ARC_OF_CIRCLE, "Unexpected entity type"); + + Quaternion q = Normal()->NormalGetNum(); + Vector u = q.RotationU(), v = q.RotationV(); + + Vector c = SK.GetEntity(point[0])->PointGetNum(); + Vector pa = SK.GetEntity(point[1])->PointGetNum(); + Vector pb = SK.GetEntity(point[2])->PointGetNum(); + + Point2d c2 = c.Project2d(u, v); + Point2d pa2 = (pa.Project2d(u, v)).Minus(c2); + Point2d pb2 = (pb.Project2d(u, v)).Minus(c2); + + *thetaa = atan2(pa2.y, pa2.x); + *thetab = atan2(pb2.y, pb2.x); + *dtheta = *thetab - *thetaa; + // If the endpoints are coincident, call it a full arc, not a zero arc; + // useful concept to have when splitting + while(*dtheta < 1e-6) *dtheta += 2*PI; + while(*dtheta > (2*PI)) *dtheta -= 2*PI; +} + +Vector EntityBase::CubicGetStartNum() const { + return SK.GetEntity(point[0])->PointGetNum(); +} +Vector EntityBase::CubicGetFinishNum() const { + return SK.GetEntity(point[3+extraPoints])->PointGetNum(); +} +ExprVector EntityBase::CubicGetStartTangentExprs() const { + ExprVector pon = SK.GetEntity(point[0])->PointGetExprs(), + poff = SK.GetEntity(point[1])->PointGetExprs(); + return (pon.Minus(poff)); +} +ExprVector EntityBase::CubicGetFinishTangentExprs() const { + ExprVector pon = SK.GetEntity(point[3+extraPoints])->PointGetExprs(), + poff = SK.GetEntity(point[2+extraPoints])->PointGetExprs(); + return (pon.Minus(poff)); +} +Vector EntityBase::CubicGetStartTangentNum() const { + Vector pon = SK.GetEntity(point[0])->PointGetNum(), + poff = SK.GetEntity(point[1])->PointGetNum(); + return (pon.Minus(poff)); +} +Vector EntityBase::CubicGetFinishTangentNum() const { + Vector pon = SK.GetEntity(point[3+extraPoints])->PointGetNum(), + poff = SK.GetEntity(point[2+extraPoints])->PointGetNum(); + return (pon.Minus(poff)); +} + +bool EntityBase::IsWorkplane() const { + return (type == Type::WORKPLANE); +} + +ExprVector EntityBase::WorkplaneGetOffsetExprs() const { + return SK.GetEntity(point[0])->PointGetExprs(); +} + +Vector EntityBase::WorkplaneGetOffset() const { + return SK.GetEntity(point[0])->PointGetNum(); +} + +void EntityBase::WorkplaneGetPlaneExprs(ExprVector *n, Expr **dn) const { + if(type == Type::WORKPLANE) { + *n = Normal()->NormalExprsN(); + + ExprVector p0 = SK.GetEntity(point[0])->PointGetExprs(); + // The plane is n dot (p - p0) = 0, or + // n dot p - n dot p0 = 0 + // so dn = n dot p0 + *dn = p0.Dot(*n); + } else ssassert(false, "Unexpected entity type"); +} + +bool EntityBase::IsDistance() const { + return (type == Type::DISTANCE) || + (type == Type::DISTANCE_N_COPY); +} +double EntityBase::DistanceGetNum() const { + if(type == Type::DISTANCE) { + return SK.GetParam(param[0])->val; + } else if(type == Type::DISTANCE_N_COPY) { + return numDistance; + } else ssassert(false, "Unexpected entity type"); +} +Expr *EntityBase::DistanceGetExpr() const { + if(type == Type::DISTANCE) { + return Expr::From(param[0]); + } else if(type == Type::DISTANCE_N_COPY) { + return Expr::From(numDistance); + } else ssassert(false, "Unexpected entity type"); +} +void EntityBase::DistanceForceTo(double v) { + if(type == Type::DISTANCE) { + (SK.GetParam(param[0]))->val = v; + } else if(type == Type::DISTANCE_N_COPY) { + // do nothing, it's locked + } else ssassert(false, "Unexpected entity type"); +} + +EntityBase *EntityBase::Normal() const { + return SK.GetEntity(normal); +} + +bool EntityBase::IsPoint() const { + switch(type) { + case Type::POINT_IN_3D: + case Type::POINT_IN_2D: + case Type::POINT_N_COPY: + case Type::POINT_N_TRANS: + case Type::POINT_N_ROT_TRANS: + case Type::POINT_N_ROT_AA: + return true; + + default: + return false; + } +} + +bool EntityBase::IsNormal() const { + switch(type) { + case Type::NORMAL_IN_3D: + case Type::NORMAL_IN_2D: + case Type::NORMAL_N_COPY: + case Type::NORMAL_N_ROT: + case Type::NORMAL_N_ROT_AA: + return true; + + default: return false; + } +} + +Quaternion EntityBase::NormalGetNum() const { + Quaternion q; + switch(type) { + case Type::NORMAL_IN_3D: + q = Quaternion::From(param[0], param[1], param[2], param[3]); + break; + + case Type::NORMAL_IN_2D: { + EntityBase *wrkpl = SK.GetEntity(workplane); + EntityBase *norm = SK.GetEntity(wrkpl->normal); + q = norm->NormalGetNum(); + break; + } + case Type::NORMAL_N_COPY: + q = numNormal; + break; + + case Type::NORMAL_N_ROT: + q = Quaternion::From(param[0], param[1], param[2], param[3]); + q = q.Times(numNormal); + break; + + case Type::NORMAL_N_ROT_AA: { + q = GetAxisAngleQuaternion(0); + q = q.Times(numNormal); + break; + } + + default: ssassert(false, "Unexpected entity type"); + } + return q; +} + +void EntityBase::NormalForceTo(Quaternion q) { + switch(type) { + case Type::NORMAL_IN_3D: + SK.GetParam(param[0])->val = q.w; + SK.GetParam(param[1])->val = q.vx; + SK.GetParam(param[2])->val = q.vy; + SK.GetParam(param[3])->val = q.vz; + break; + + case Type::NORMAL_IN_2D: + case Type::NORMAL_N_COPY: + // There's absolutely nothing to do; these are locked. + break; + case Type::NORMAL_N_ROT: { + Quaternion qp = q.Times(numNormal.Inverse()); + + SK.GetParam(param[0])->val = qp.w; + SK.GetParam(param[1])->val = qp.vx; + SK.GetParam(param[2])->val = qp.vy; + SK.GetParam(param[3])->val = qp.vz; + break; + } + + case Type::NORMAL_N_ROT_AA: + // Not sure if I'll bother implementing this one + break; + + default: ssassert(false, "Unexpected entity type"); + } +} + +Vector EntityBase::NormalU() const { + return NormalGetNum().RotationU(); +} +Vector EntityBase::NormalV() const { + return NormalGetNum().RotationV(); +} +Vector EntityBase::NormalN() const { + return NormalGetNum().RotationN(); +} + +ExprVector EntityBase::NormalExprsU() const { + return NormalGetExprs().RotationU(); +} +ExprVector EntityBase::NormalExprsV() const { + return NormalGetExprs().RotationV(); +} +ExprVector EntityBase::NormalExprsN() const { + return NormalGetExprs().RotationN(); +} + +ExprQuaternion EntityBase::NormalGetExprs() const { + ExprQuaternion q; + switch(type) { + case Type::NORMAL_IN_3D: + q = ExprQuaternion::From(param[0], param[1], param[2], param[3]); + break; + + case Type::NORMAL_IN_2D: { + EntityBase *wrkpl = SK.GetEntity(workplane); + EntityBase *norm = SK.GetEntity(wrkpl->normal); + q = norm->NormalGetExprs(); + break; + } + case Type::NORMAL_N_COPY: + q = ExprQuaternion::From(numNormal); + break; + + case Type::NORMAL_N_ROT: { + ExprQuaternion orig = ExprQuaternion::From(numNormal); + q = ExprQuaternion::From(param[0], param[1], param[2], param[3]); + + q = q.Times(orig); + break; + } + + case Type::NORMAL_N_ROT_AA: { + ExprQuaternion orig = ExprQuaternion::From(numNormal); + q = GetAxisAngleQuaternionExprs(0); + q = q.Times(orig); + break; + } + + default: ssassert(false, "Unexpected entity type"); + } + return q; +} + +void EntityBase::PointForceParamTo(Vector p) { + switch(type) { + case Type::POINT_IN_3D: + SK.GetParam(param[0])->val = p.x; + SK.GetParam(param[1])->val = p.y; + SK.GetParam(param[2])->val = p.z; + break; + + case Type::POINT_IN_2D: + SK.GetParam(param[0])->val = p.x; + SK.GetParam(param[1])->val = p.y; + break; + + default: ssassert(false, "Unexpected entity type"); + } +} + +void EntityBase::PointForceTo(Vector p) { + switch(type) { + case Type::POINT_IN_3D: + SK.GetParam(param[0])->val = p.x; + SK.GetParam(param[1])->val = p.y; + SK.GetParam(param[2])->val = p.z; + break; + + case Type::POINT_IN_2D: { + EntityBase *c = SK.GetEntity(workplane); + p = p.Minus(c->WorkplaneGetOffset()); + SK.GetParam(param[0])->val = p.Dot(c->Normal()->NormalU()); + SK.GetParam(param[1])->val = p.Dot(c->Normal()->NormalV()); + break; + } + + case Type::POINT_N_TRANS: { + if(timesApplied == 0) break; + Vector trans = (p.Minus(numPoint)).ScaledBy(1.0/timesApplied); + SK.GetParam(param[0])->val = trans.x; + SK.GetParam(param[1])->val = trans.y; + SK.GetParam(param[2])->val = trans.z; + break; + } + + case Type::POINT_N_ROT_TRANS: { + // Force only the translation; leave the rotation unchanged. But + // remember that we're working with respect to the rotated + // point. + Vector trans = p.Minus(PointGetQuaternion().Rotate(numPoint)); + SK.GetParam(param[0])->val = trans.x; + SK.GetParam(param[1])->val = trans.y; + SK.GetParam(param[2])->val = trans.z; + break; + } + + case Type::POINT_N_ROT_AA: { + // Force only the angle; the axis and center of rotation stay + Vector offset = Vector::From(param[0], param[1], param[2]); + Vector normal = Vector::From(param[4], param[5], param[6]); + Vector u = normal.Normal(0), v = normal.Normal(1); + Vector po = p.Minus(offset), numo = numPoint.Minus(offset); + double thetap = atan2(v.Dot(po), u.Dot(po)); + double thetan = atan2(v.Dot(numo), u.Dot(numo)); + double thetaf = (thetap - thetan); + double thetai = (SK.GetParam(param[3])->val)*timesApplied*2; + double dtheta = thetaf - thetai; + // Take the smallest possible change in the actual step angle, + // in order to avoid jumps when you cross from +pi to -pi + while(dtheta < -PI) dtheta += 2*PI; + while(dtheta > PI) dtheta -= 2*PI; + SK.GetParam(param[3])->val = (thetai + dtheta)/(timesApplied*2); + break; + } + + case Type::POINT_N_COPY: + // Nothing to do; it's a static copy + break; + + default: ssassert(false, "Unexpected entity type"); + } +} + +Vector EntityBase::PointGetNum() const { + Vector p; + switch(type) { + case Type::POINT_IN_3D: + p = Vector::From(param[0], param[1], param[2]); + break; + + case Type::POINT_IN_2D: { + EntityBase *c = SK.GetEntity(workplane); + Vector u = c->Normal()->NormalU(); + Vector v = c->Normal()->NormalV(); + p = u.ScaledBy(SK.GetParam(param[0])->val); + p = p.Plus(v.ScaledBy(SK.GetParam(param[1])->val)); + p = p.Plus(c->WorkplaneGetOffset()); + break; + } + + case Type::POINT_N_TRANS: { + Vector trans = Vector::From(param[0], param[1], param[2]); + p = numPoint.Plus(trans.ScaledBy(timesApplied)); + break; + } + + case Type::POINT_N_ROT_TRANS: { + Vector offset = Vector::From(param[0], param[1], param[2]); + Quaternion q = PointGetQuaternion(); + p = q.Rotate(numPoint); + p = p.Plus(offset); + break; + } + + case Type::POINT_N_ROT_AA: { + Vector offset = Vector::From(param[0], param[1], param[2]); + Quaternion q = PointGetQuaternion(); + p = numPoint.Minus(offset); + p = q.Rotate(p); + p = p.Plus(offset); + break; + } + + case Type::POINT_N_COPY: + p = numPoint; + break; + + default: ssassert(false, "Unexpected entity type"); + } + return p; +} + +ExprVector EntityBase::PointGetExprs() const { + ExprVector r; + switch(type) { + case Type::POINT_IN_3D: + r = ExprVector::From(param[0], param[1], param[2]); + break; + + case Type::POINT_IN_2D: { + EntityBase *c = SK.GetEntity(workplane); + ExprVector u = c->Normal()->NormalExprsU(); + ExprVector v = c->Normal()->NormalExprsV(); + r = c->WorkplaneGetOffsetExprs(); + r = r.Plus(u.ScaledBy(Expr::From(param[0]))); + r = r.Plus(v.ScaledBy(Expr::From(param[1]))); + break; + } + case Type::POINT_N_TRANS: { + ExprVector orig = ExprVector::From(numPoint); + ExprVector trans = ExprVector::From(param[0], param[1], param[2]); + r = orig.Plus(trans.ScaledBy(Expr::From(timesApplied))); + break; + } + case Type::POINT_N_ROT_TRANS: { + ExprVector orig = ExprVector::From(numPoint); + ExprVector trans = ExprVector::From(param[0], param[1], param[2]); + ExprQuaternion q = + ExprQuaternion::From(param[3], param[4], param[5], param[6]); + orig = q.Rotate(orig); + r = orig.Plus(trans); + break; + } + case Type::POINT_N_ROT_AA: { + ExprVector orig = ExprVector::From(numPoint); + ExprVector trans = ExprVector::From(param[0], param[1], param[2]); + ExprQuaternion q = GetAxisAngleQuaternionExprs(3); + orig = orig.Minus(trans); + orig = q.Rotate(orig); + r = orig.Plus(trans); + break; + } + case Type::POINT_N_COPY: + r = ExprVector::From(numPoint); + break; + + default: ssassert(false, "Unexpected entity type"); + } + return r; +} + +void EntityBase::PointGetExprsInWorkplane(hEntity wrkpl, Expr **u, Expr **v) const { + if(type == Type::POINT_IN_2D && workplane.v == wrkpl.v) { + // They want our coordinates in the form that we've written them, + // very nice. + *u = Expr::From(param[0]); + *v = Expr::From(param[1]); + } else { + // Get the offset and basis vectors for this weird exotic csys. + EntityBase *w = SK.GetEntity(wrkpl); + ExprVector wp = w->WorkplaneGetOffsetExprs(); + ExprVector wu = w->Normal()->NormalExprsU(); + ExprVector wv = w->Normal()->NormalExprsV(); + + // Get our coordinates in three-space, and project them into that + // coordinate system. + ExprVector ev = PointGetExprs(); + ev = ev.Minus(wp); + *u = ev.Dot(wu); + *v = ev.Dot(wv); + } +} + +ExprVector EntityBase::PointGetExprsInWorkplane(hEntity wrkpl) const { + if(wrkpl.v == Entity::FREE_IN_3D.v) { + return PointGetExprs(); + } + + ExprVector r; + PointGetExprsInWorkplane(wrkpl, &r.x, &r.y); + r.z = Expr::From(0.0); + return r; +} + +void EntityBase::PointForceQuaternionTo(Quaternion q) { + ssassert(type == Type::POINT_N_ROT_TRANS, "Unexpected entity type"); + + SK.GetParam(param[3])->val = q.w; + SK.GetParam(param[4])->val = q.vx; + SK.GetParam(param[5])->val = q.vy; + SK.GetParam(param[6])->val = q.vz; +} + +Quaternion EntityBase::GetAxisAngleQuaternion(int param0) const { + Quaternion q; + double theta = timesApplied*SK.GetParam(param[param0+0])->val; + double s = sin(theta), c = cos(theta); + q.w = c; + q.vx = s*SK.GetParam(param[param0+1])->val; + q.vy = s*SK.GetParam(param[param0+2])->val; + q.vz = s*SK.GetParam(param[param0+3])->val; + return q; +} + +ExprQuaternion EntityBase::GetAxisAngleQuaternionExprs(int param0) const { + ExprQuaternion q; + + Expr *theta = Expr::From(timesApplied)->Times( + Expr::From(param[param0+0])); + Expr *c = theta->Cos(), *s = theta->Sin(); + q.w = c; + q.vx = s->Times(Expr::From(param[param0+1])); + q.vy = s->Times(Expr::From(param[param0+2])); + q.vz = s->Times(Expr::From(param[param0+3])); + return q; +} + +Quaternion EntityBase::PointGetQuaternion() const { + Quaternion q; + + if(type == Type::POINT_N_ROT_AA) { + q = GetAxisAngleQuaternion(3); + } else if(type == Type::POINT_N_ROT_TRANS) { + q = Quaternion::From(param[3], param[4], param[5], param[6]); + } else ssassert(false, "Unexpected entity type"); + + return q; +} + +bool EntityBase::IsFace() const { + switch(type) { + case Type::FACE_NORMAL_PT: + case Type::FACE_XPROD: + case Type::FACE_N_ROT_TRANS: + case Type::FACE_N_TRANS: + case Type::FACE_N_ROT_AA: + return true; + default: + return false; + } +} + +ExprVector EntityBase::FaceGetNormalExprs() const { + ExprVector r; + if(type == Type::FACE_NORMAL_PT) { + Vector v = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz); + r = ExprVector::From(v.WithMagnitude(1)); + } else if(type == Type::FACE_XPROD) { + ExprVector vc = ExprVector::From(param[0], param[1], param[2]); + ExprVector vn = + ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz); + r = vc.Cross(vn); + r = r.WithMagnitude(Expr::From(1.0)); + } else if(type == Type::FACE_N_ROT_TRANS) { + // The numerical normal vector gets the rotation; the numerical + // normal has magnitude one, and the rotation doesn't change that, + // so there's no need to fix it up. + r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz); + ExprQuaternion q = + ExprQuaternion::From(param[3], param[4], param[5], param[6]); + r = q.Rotate(r); + } else if(type == Type::FACE_N_TRANS) { + r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz); + } else if(type == Type::FACE_N_ROT_AA) { + r = ExprVector::From(numNormal.vx, numNormal.vy, numNormal.vz); + ExprQuaternion q = GetAxisAngleQuaternionExprs(3); + r = q.Rotate(r); + } else ssassert(false, "Unexpected entity type"); + return r; +} + +Vector EntityBase::FaceGetNormalNum() const { + Vector r; + if(type == Type::FACE_NORMAL_PT) { + r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz); + } else if(type == Type::FACE_XPROD) { + Vector vc = Vector::From(param[0], param[1], param[2]); + Vector vn = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz); + r = vc.Cross(vn); + } else if(type == Type::FACE_N_ROT_TRANS) { + // The numerical normal vector gets the rotation + r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz); + Quaternion q = Quaternion::From(param[3], param[4], param[5], param[6]); + r = q.Rotate(r); + } else if(type == Type::FACE_N_TRANS) { + r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz); + } else if(type == Type::FACE_N_ROT_AA) { + r = Vector::From(numNormal.vx, numNormal.vy, numNormal.vz); + Quaternion q = GetAxisAngleQuaternion(3); + r = q.Rotate(r); + } else ssassert(false, "Unexpected entity type"); + return r.WithMagnitude(1); +} + +ExprVector EntityBase::FaceGetPointExprs() const { + ExprVector r; + if(type == Type::FACE_NORMAL_PT) { + r = SK.GetEntity(point[0])->PointGetExprs(); + } else if(type == Type::FACE_XPROD) { + r = ExprVector::From(numPoint); + } else if(type == Type::FACE_N_ROT_TRANS) { + // The numerical point gets the rotation and translation. + ExprVector trans = ExprVector::From(param[0], param[1], param[2]); + ExprQuaternion q = + ExprQuaternion::From(param[3], param[4], param[5], param[6]); + r = ExprVector::From(numPoint); + r = q.Rotate(r); + r = r.Plus(trans); + } else if(type == Type::FACE_N_TRANS) { + ExprVector trans = ExprVector::From(param[0], param[1], param[2]); + r = ExprVector::From(numPoint); + r = r.Plus(trans.ScaledBy(Expr::From(timesApplied))); + } else if(type == Type::FACE_N_ROT_AA) { + ExprVector trans = ExprVector::From(param[0], param[1], param[2]); + ExprQuaternion q = GetAxisAngleQuaternionExprs(3); + r = ExprVector::From(numPoint); + r = r.Minus(trans); + r = q.Rotate(r); + r = r.Plus(trans); + } else ssassert(false, "Unexpected entity type"); + return r; +} + +Vector EntityBase::FaceGetPointNum() const { + Vector r; + if(type == Type::FACE_NORMAL_PT) { + r = SK.GetEntity(point[0])->PointGetNum(); + } else if(type == Type::FACE_XPROD) { + r = numPoint; + } else if(type == Type::FACE_N_ROT_TRANS) { + // The numerical point gets the rotation and translation. + Vector trans = Vector::From(param[0], param[1], param[2]); + Quaternion q = Quaternion::From(param[3], param[4], param[5], param[6]); + r = q.Rotate(numPoint); + r = r.Plus(trans); + } else if(type == Type::FACE_N_TRANS) { + Vector trans = Vector::From(param[0], param[1], param[2]); + r = numPoint.Plus(trans.ScaledBy(timesApplied)); + } else if(type == Type::FACE_N_ROT_AA) { + Vector trans = Vector::From(param[0], param[1], param[2]); + Quaternion q = GetAxisAngleQuaternion(3); + r = numPoint.Minus(trans); + r = q.Rotate(r); + r = r.Plus(trans); + } else ssassert(false, "Unexpected entity type"); + return r; +} + +bool EntityBase::HasEndpoints() const { + return (type == Type::LINE_SEGMENT) || + (type == Type::CUBIC) || + (type == Type::ARC_OF_CIRCLE); +} +Vector EntityBase::EndpointStart() const { + if(type == Type::LINE_SEGMENT) { + return SK.GetEntity(point[0])->PointGetNum(); + } else if(type == Type::CUBIC) { + return CubicGetStartNum(); + } else if(type == Type::ARC_OF_CIRCLE) { + return SK.GetEntity(point[1])->PointGetNum(); + } else ssassert(false, "Unexpected entity type"); +} +Vector EntityBase::EndpointFinish() const { + if(type == Type::LINE_SEGMENT) { + return SK.GetEntity(point[1])->PointGetNum(); + } else if(type == Type::CUBIC) { + return CubicGetFinishNum(); + } else if(type == Type::ARC_OF_CIRCLE) { + return SK.GetEntity(point[2])->PointGetNum(); + } else ssassert(false, "Unexpected entity type"); +} + +void EntityBase::RectGetPointsExprs(ExprVector *eb, ExprVector *ec) const { + ssassert(type == Type::TTF_TEXT || type == Type::IMAGE, + "Unexpected entity type"); + + EntityBase *a = SK.GetEntity(point[0]); + EntityBase *o = SK.GetEntity(point[1]); + + // Write equations for each point in the current workplane. + // This reduces the complexity of resulting equations. + ExprVector ea = a->PointGetExprsInWorkplane(workplane); + ExprVector eo = o->PointGetExprsInWorkplane(workplane); + + // Take perpendicular vector and scale it by aspect ratio. + ExprVector eu = ea.Minus(eo); + ExprVector ev = ExprVector::From(eu.y, eu.x->Negate(), eu.z).ScaledBy(Expr::From(aspectRatio)); + + *eb = eo.Plus(ev); + *ec = eo.Plus(eu).Plus(ev); +} + +void EntityBase::AddEq(IdList *l, Expr *expr, int index) const { + Equation eq; + eq.e = expr; + eq.h = h.equation(index); + l->Add(&eq); +} + +void EntityBase::GenerateEquations(IdList *l) const { + switch(type) { + case Type::NORMAL_IN_3D: { + ExprQuaternion q = NormalGetExprs(); + AddEq(l, (q.Magnitude())->Minus(Expr::From(1)), 0); + break; + } + + case Type::ARC_OF_CIRCLE: { + // If this is a copied entity, with its point already fixed + // with respect to each other, then we don't want to generate + // the distance constraint! + if(SK.GetEntity(point[0])->type != Type::POINT_IN_2D) break; + + // If the two endpoints of the arc are constrained coincident + // (to make a complete circle), then our distance constraint + // would be redundant and therefore overconstrain things. + int i; + for(i = 0; i < SK.constraint.n; i++) { + ConstraintBase *c = &(SK.constraint.elem[i]); + if(c->group.v != group.v) continue; + if(c->type != Constraint::Type::POINTS_COINCIDENT) continue; + + if((c->ptA.v == point[1].v && c->ptB.v == point[2].v) || + (c->ptA.v == point[2].v && c->ptB.v == point[1].v)) + { + break; + } + } + if(i < SK.constraint.n) break; + + Expr *ra = Constraint::Distance(workplane, point[0], point[1]); + Expr *rb = Constraint::Distance(workplane, point[0], point[2]); + AddEq(l, ra->Minus(rb), 0); + break; + } + + case Type::IMAGE: + case Type::TTF_TEXT: { + if(SK.GetEntity(point[0])->type != Type::POINT_IN_2D) break; + EntityBase *b = SK.GetEntity(point[2]); + EntityBase *c = SK.GetEntity(point[3]); + ExprVector eb = b->PointGetExprsInWorkplane(workplane); + ExprVector ec = c->PointGetExprsInWorkplane(workplane); + + ExprVector ebp, ecp; + RectGetPointsExprs(&ebp, &ecp); + + ExprVector beq = eb.Minus(ebp); + AddEq(l, beq.x, 0); + AddEq(l, beq.y, 1); + ExprVector ceq = ec.Minus(ecp); + AddEq(l, ceq.x, 2); + AddEq(l, ceq.y, 3); + break; + } + + default: // Most entities do not generate equations. + break; + } +} diff --git a/ThirdParty/libslvs/expr.cpp b/ThirdParty/libslvs/expr.cpp new file mode 100644 index 0000000000..cbff038515 --- /dev/null +++ b/ThirdParty/libslvs/expr.cpp @@ -0,0 +1,917 @@ +//----------------------------------------------------------------------------- +// The symbolic algebra system used to write our constraint equations; +// routines to build expressions in software or from a user-provided string, +// and to compute the partial derivatives that we'll use when write our +// Jacobian matrix. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- +#include "solvespace.h" + +ExprVector ExprVector::From(Expr *x, Expr *y, Expr *z) { + ExprVector r = { x, y, z}; + return r; +} + +ExprVector ExprVector::From(Vector vn) { + ExprVector ve; + ve.x = Expr::From(vn.x); + ve.y = Expr::From(vn.y); + ve.z = Expr::From(vn.z); + return ve; +} + +ExprVector ExprVector::From(hParam x, hParam y, hParam z) { + ExprVector ve; + ve.x = Expr::From(x); + ve.y = Expr::From(y); + ve.z = Expr::From(z); + return ve; +} + +ExprVector ExprVector::From(double x, double y, double z) { + ExprVector ve; + ve.x = Expr::From(x); + ve.y = Expr::From(y); + ve.z = Expr::From(z); + return ve; +} + +ExprVector ExprVector::Minus(ExprVector b) const { + ExprVector r; + r.x = x->Minus(b.x); + r.y = y->Minus(b.y); + r.z = z->Minus(b.z); + return r; +} + +ExprVector ExprVector::Plus(ExprVector b) const { + ExprVector r; + r.x = x->Plus(b.x); + r.y = y->Plus(b.y); + r.z = z->Plus(b.z); + return r; +} + +Expr *ExprVector::Dot(ExprVector b) const { + Expr *r; + r = x->Times(b.x); + r = r->Plus(y->Times(b.y)); + r = r->Plus(z->Times(b.z)); + return r; +} + +ExprVector ExprVector::Cross(ExprVector b) const { + ExprVector r; + r.x = (y->Times(b.z))->Minus(z->Times(b.y)); + r.y = (z->Times(b.x))->Minus(x->Times(b.z)); + r.z = (x->Times(b.y))->Minus(y->Times(b.x)); + return r; +} + +ExprVector ExprVector::ScaledBy(Expr *s) const { + ExprVector r; + r.x = x->Times(s); + r.y = y->Times(s); + r.z = z->Times(s); + return r; +} + +ExprVector ExprVector::WithMagnitude(Expr *s) const { + Expr *m = Magnitude(); + return ScaledBy(s->Div(m)); +} + +Expr *ExprVector::Magnitude() const { + Expr *r; + r = x->Square(); + r = r->Plus(y->Square()); + r = r->Plus(z->Square()); + return r->Sqrt(); +} + +Vector ExprVector::Eval() const { + Vector r; + r.x = x->Eval(); + r.y = y->Eval(); + r.z = z->Eval(); + return r; +} + +ExprQuaternion ExprQuaternion::From(hParam w, hParam vx, hParam vy, hParam vz) { + ExprQuaternion q; + q.w = Expr::From(w); + q.vx = Expr::From(vx); + q.vy = Expr::From(vy); + q.vz = Expr::From(vz); + return q; +} + +ExprQuaternion ExprQuaternion::From(Expr *w, Expr *vx, Expr *vy, Expr *vz) +{ + ExprQuaternion q; + q.w = w; + q.vx = vx; + q.vy = vy; + q.vz = vz; + return q; +} + +ExprQuaternion ExprQuaternion::From(Quaternion qn) { + ExprQuaternion qe; + qe.w = Expr::From(qn.w); + qe.vx = Expr::From(qn.vx); + qe.vy = Expr::From(qn.vy); + qe.vz = Expr::From(qn.vz); + return qe; +} + +ExprVector ExprQuaternion::RotationU() const { + ExprVector u; + Expr *two = Expr::From(2); + + u.x = w->Square(); + u.x = (u.x)->Plus(vx->Square()); + u.x = (u.x)->Minus(vy->Square()); + u.x = (u.x)->Minus(vz->Square()); + + u.y = two->Times(w->Times(vz)); + u.y = (u.y)->Plus(two->Times(vx->Times(vy))); + + u.z = two->Times(vx->Times(vz)); + u.z = (u.z)->Minus(two->Times(w->Times(vy))); + + return u; +} + +ExprVector ExprQuaternion::RotationV() const { + ExprVector v; + Expr *two = Expr::From(2); + + v.x = two->Times(vx->Times(vy)); + v.x = (v.x)->Minus(two->Times(w->Times(vz))); + + v.y = w->Square(); + v.y = (v.y)->Minus(vx->Square()); + v.y = (v.y)->Plus(vy->Square()); + v.y = (v.y)->Minus(vz->Square()); + + v.z = two->Times(w->Times(vx)); + v.z = (v.z)->Plus(two->Times(vy->Times(vz))); + + return v; +} + +ExprVector ExprQuaternion::RotationN() const { + ExprVector n; + Expr *two = Expr::From(2); + + n.x = two->Times( w->Times(vy)); + n.x = (n.x)->Plus (two->Times(vx->Times(vz))); + + n.y = two->Times(vy->Times(vz)); + n.y = (n.y)->Minus(two->Times( w->Times(vx))); + + n.z = w->Square(); + n.z = (n.z)->Minus(vx->Square()); + n.z = (n.z)->Minus(vy->Square()); + n.z = (n.z)->Plus (vz->Square()); + + return n; +} + +ExprVector ExprQuaternion::Rotate(ExprVector p) const { + // Express the point in the new basis + return (RotationU().ScaledBy(p.x)).Plus( + RotationV().ScaledBy(p.y)).Plus( + RotationN().ScaledBy(p.z)); +} + +ExprQuaternion ExprQuaternion::Times(ExprQuaternion b) const { + Expr *sa = w, *sb = b.w; + ExprVector va = { vx, vy, vz }; + ExprVector vb = { b.vx, b.vy, b.vz }; + + ExprQuaternion r; + r.w = (sa->Times(sb))->Minus(va.Dot(vb)); + ExprVector vr = vb.ScaledBy(sa).Plus( + va.ScaledBy(sb).Plus( + va.Cross(vb))); + r.vx = vr.x; + r.vy = vr.y; + r.vz = vr.z; + return r; +} + +Expr *ExprQuaternion::Magnitude() const { + return ((w ->Square())->Plus( + (vx->Square())->Plus( + (vy->Square())->Plus( + (vz->Square())))))->Sqrt(); +} + + +Expr *Expr::From(hParam p) { + Expr *r = AllocExpr(); + r->op = Op::PARAM; + r->parh = p; + return r; +} + +Expr *Expr::From(double v) { + // Statically allocate common constants. + // Note: this is only valid because AllocExpr() uses AllocTemporary(), + // and Expr* is never explicitly freed. + + if(v == 0.0) { + static Expr zero(0.0); + return &zero; + } + + if(v == 1.0) { + static Expr one(1.0); + return &one; + } + + if(v == -1.0) { + static Expr mone(-1.0); + return &mone; + } + + if(v == 0.5) { + static Expr half(0.5); + return ½ + } + + if(v == -0.5) { + static Expr mhalf(-0.5); + return &mhalf; + } + + Expr *r = AllocExpr(); + r->op = Op::CONSTANT; + r->v = v; + return r; +} + +Expr *Expr::AnyOp(Op newOp, Expr *b) { + Expr *r = AllocExpr(); + r->op = newOp; + r->a = this; + r->b = b; + return r; +} + +int Expr::Children() const { + switch(op) { + case Op::PARAM: + case Op::PARAM_PTR: + case Op::CONSTANT: + case Op::VARIABLE: + return 0; + + case Op::PLUS: + case Op::MINUS: + case Op::TIMES: + case Op::DIV: + return 2; + + case Op::NEGATE: + case Op::SQRT: + case Op::SQUARE: + case Op::SIN: + case Op::COS: + case Op::ASIN: + case Op::ACOS: + return 1; + } + ssassert(false, "Unexpected operation"); +} + +int Expr::Nodes() const { + switch(Children()) { + case 0: return 1; + case 1: return 1 + a->Nodes(); + case 2: return 1 + a->Nodes() + b->Nodes(); + default: ssassert(false, "Unexpected children count"); + } +} + +Expr *Expr::DeepCopy() const { + Expr *n = AllocExpr(); + *n = *this; + int c = n->Children(); + if(c > 0) n->a = a->DeepCopy(); + if(c > 1) n->b = b->DeepCopy(); + return n; +} + +Expr *Expr::DeepCopyWithParamsAsPointers(IdList *firstTry, + IdList *thenTry) const +{ + Expr *n = AllocExpr(); + if(op == Op::PARAM) { + // A param that is referenced by its hParam gets rewritten to go + // straight in to the parameter table with a pointer, or simply + // into a constant if it's already known. + Param *p = firstTry->FindByIdNoOops(parh); + if(!p) p = thenTry->FindById(parh); + if(p->known) { + n->op = Op::CONSTANT; + n->v = p->val; + } else { + n->op = Op::PARAM_PTR; + n->parp = p; + } + return n; + } + + *n = *this; + int c = n->Children(); + if(c > 0) n->a = a->DeepCopyWithParamsAsPointers(firstTry, thenTry); + if(c > 1) n->b = b->DeepCopyWithParamsAsPointers(firstTry, thenTry); + return n; +} + +double Expr::Eval() const { + switch(op) { + case Op::PARAM: return SK.GetParam(parh)->val; + case Op::PARAM_PTR: return parp->val; + + case Op::CONSTANT: return v; + case Op::VARIABLE: ssassert(false, "Not supported yet"); + + case Op::PLUS: return a->Eval() + b->Eval(); + case Op::MINUS: return a->Eval() - b->Eval(); + case Op::TIMES: return a->Eval() * b->Eval(); + case Op::DIV: return a->Eval() / b->Eval(); + + case Op::NEGATE: return -(a->Eval()); + case Op::SQRT: return sqrt(a->Eval()); + case Op::SQUARE: { double r = a->Eval(); return r*r; } + case Op::SIN: return sin(a->Eval()); + case Op::COS: return cos(a->Eval()); + case Op::ACOS: return acos(a->Eval()); + case Op::ASIN: return asin(a->Eval()); + } + ssassert(false, "Unexpected operation"); +} + +Expr *Expr::PartialWrt(hParam p) const { + Expr *da, *db; + + switch(op) { + case Op::PARAM_PTR: return From(p.v == parp->h.v ? 1 : 0); + case Op::PARAM: return From(p.v == parh.v ? 1 : 0); + + case Op::CONSTANT: return From(0.0); + case Op::VARIABLE: ssassert(false, "Not supported yet"); + + case Op::PLUS: return (a->PartialWrt(p))->Plus(b->PartialWrt(p)); + case Op::MINUS: return (a->PartialWrt(p))->Minus(b->PartialWrt(p)); + + case Op::TIMES: + da = a->PartialWrt(p); + db = b->PartialWrt(p); + return (a->Times(db))->Plus(b->Times(da)); + + case Op::DIV: + da = a->PartialWrt(p); + db = b->PartialWrt(p); + return ((da->Times(b))->Minus(a->Times(db)))->Div(b->Square()); + + case Op::SQRT: + return (From(0.5)->Div(a->Sqrt()))->Times(a->PartialWrt(p)); + + case Op::SQUARE: + return (From(2.0)->Times(a))->Times(a->PartialWrt(p)); + + case Op::NEGATE: return (a->PartialWrt(p))->Negate(); + case Op::SIN: return (a->Cos())->Times(a->PartialWrt(p)); + case Op::COS: return ((a->Sin())->Times(a->PartialWrt(p)))->Negate(); + + case Op::ASIN: + return (From(1)->Div((From(1)->Minus(a->Square()))->Sqrt())) + ->Times(a->PartialWrt(p)); + case Op::ACOS: + return (From(-1)->Div((From(1)->Minus(a->Square()))->Sqrt())) + ->Times(a->PartialWrt(p)); + } + ssassert(false, "Unexpected operation"); +} + +uint64_t Expr::ParamsUsed() const { + uint64_t r = 0; + if(op == Op::PARAM) r |= ((uint64_t)1 << (parh.v % 61)); + if(op == Op::PARAM_PTR) r |= ((uint64_t)1 << (parp->h.v % 61)); + + int c = Children(); + if(c >= 1) r |= a->ParamsUsed(); + if(c >= 2) r |= b->ParamsUsed(); + return r; +} + +bool Expr::DependsOn(hParam p) const { + if(op == Op::PARAM) return (parh.v == p.v); + if(op == Op::PARAM_PTR) return (parp->h.v == p.v); + + int c = Children(); + if(c == 1) return a->DependsOn(p); + if(c == 2) return a->DependsOn(p) || b->DependsOn(p); + return false; +} + +bool Expr::Tol(double a, double b) { + return fabs(a - b) < 0.001; +} +Expr *Expr::FoldConstants() { + Expr *n = AllocExpr(); + *n = *this; + + int c = Children(); + if(c >= 1) n->a = a->FoldConstants(); + if(c >= 2) n->b = b->FoldConstants(); + + switch(op) { + case Op::PARAM_PTR: + case Op::PARAM: + case Op::CONSTANT: + case Op::VARIABLE: + break; + + case Op::MINUS: + case Op::TIMES: + case Op::DIV: + case Op::PLUS: + // If both ops are known, then we can evaluate immediately + if(n->a->op == Op::CONSTANT && n->b->op == Op::CONSTANT) { + double nv = n->Eval(); + n->op = Op::CONSTANT; + n->v = nv; + break; + } + // x + 0 = 0 + x = x + if(op == Op::PLUS && n->b->op == Op::CONSTANT && Tol(n->b->v, 0)) { + *n = *(n->a); break; + } + if(op == Op::PLUS && n->a->op == Op::CONSTANT && Tol(n->a->v, 0)) { + *n = *(n->b); break; + } + // 1*x = x*1 = x + if(op == Op::TIMES && n->b->op == Op::CONSTANT && Tol(n->b->v, 1)) { + *n = *(n->a); break; + } + if(op == Op::TIMES && n->a->op == Op::CONSTANT && Tol(n->a->v, 1)) { + *n = *(n->b); break; + } + // 0*x = x*0 = 0 + if(op == Op::TIMES && n->b->op == Op::CONSTANT && Tol(n->b->v, 0)) { + n->op = Op::CONSTANT; n->v = 0; break; + } + if(op == Op::TIMES && n->a->op == Op::CONSTANT && Tol(n->a->v, 0)) { + n->op = Op::CONSTANT; n->v = 0; break; + } + + break; + + case Op::SQRT: + case Op::SQUARE: + case Op::NEGATE: + case Op::SIN: + case Op::COS: + case Op::ASIN: + case Op::ACOS: + if(n->a->op == Op::CONSTANT) { + double nv = n->Eval(); + n->op = Op::CONSTANT; + n->v = nv; + } + break; + } + return n; +} + +void Expr::Substitute(hParam oldh, hParam newh) { + ssassert(op != Op::PARAM_PTR, "Expected an expression that refer to params via handles"); + + if(op == Op::PARAM && parh.v == oldh.v) { + parh = newh; + } + int c = Children(); + if(c >= 1) a->Substitute(oldh, newh); + if(c >= 2) b->Substitute(oldh, newh); +} + +//----------------------------------------------------------------------------- +// If the expression references only one parameter that appears in pl, then +// return that parameter. If no param is referenced, then return NO_PARAMS. +// If multiple params are referenced, then return MULTIPLE_PARAMS. +//----------------------------------------------------------------------------- +const hParam Expr::NO_PARAMS = { 0 }; +const hParam Expr::MULTIPLE_PARAMS = { 1 }; +hParam Expr::ReferencedParams(ParamList *pl) const { + if(op == Op::PARAM) { + if(pl->FindByIdNoOops(parh)) { + return parh; + } else { + return NO_PARAMS; + } + } + ssassert(op != Op::PARAM_PTR, "Expected an expression that refer to params via handles"); + + int c = Children(); + if(c == 0) { + return NO_PARAMS; + } else if(c == 1) { + return a->ReferencedParams(pl); + } else if(c == 2) { + hParam pa, pb; + pa = a->ReferencedParams(pl); + pb = b->ReferencedParams(pl); + if(pa.v == NO_PARAMS.v) { + return pb; + } else if(pb.v == NO_PARAMS.v) { + return pa; + } else if(pa.v == pb.v) { + return pa; // either, doesn't matter + } else { + return MULTIPLE_PARAMS; + } + } else ssassert(false, "Unexpected children count"); +} + + +//----------------------------------------------------------------------------- +// Routines to pretty-print an expression. Mostly for debugging. +//----------------------------------------------------------------------------- + +std::string Expr::Print() const { + char c; + switch(op) { + case Op::PARAM: return ssprintf("param(%08x)", parh.v); + case Op::PARAM_PTR: return ssprintf("param(p%08x)", parp->h.v); + + case Op::CONSTANT: return ssprintf("%.3f", v); + case Op::VARIABLE: return "(var)"; + + case Op::PLUS: c = '+'; goto p; + case Op::MINUS: c = '-'; goto p; + case Op::TIMES: c = '*'; goto p; + case Op::DIV: c = '/'; goto p; +p: + return "(" + a->Print() + " " + c + " " + b->Print() + ")"; + break; + + case Op::NEGATE: return "(- " + a->Print() + ")"; + case Op::SQRT: return "(sqrt " + a->Print() + ")"; + case Op::SQUARE: return "(square " + a->Print() + ")"; + case Op::SIN: return "(sin " + a->Print() + ")"; + case Op::COS: return "(cos " + a->Print() + ")"; + case Op::ASIN: return "(asin " + a->Print() + ")"; + case Op::ACOS: return "(acos " + a->Print() + ")"; + } + ssassert(false, "Unexpected operation"); +} + + +//----------------------------------------------------------------------------- +// A parser; convert a string to an expression. Infix notation, with the +// usual shift/reduce approach. I had great hopes for user-entered eq +// constraints, but those don't seem very useful, so right now this is just +// to provide calculator type functionality wherever numbers are entered. +//----------------------------------------------------------------------------- + +class ExprParser { +public: + enum class TokenType { + ERROR = 0, + + PAREN_LEFT, + PAREN_RIGHT, + BINARY_OP, + UNARY_OP, + OPERAND, + + END, + }; + + class Token { + public: + TokenType type; + Expr *expr; + + static Token From(TokenType type = TokenType::ERROR, Expr *expr = NULL); + static Token From(TokenType type, Expr::Op op); + bool IsError() const { return type == TokenType::ERROR; } + }; + + const char *input; + unsigned inputPos; + std::vector stack; + + char ReadChar(); + char PeekChar(); + + std::string ReadWord(); + void SkipSpace(); + + Token PopOperator(std::string *error); + Token PopOperand(std::string *error); + + int Precedence(Token token); + Token LexNumber(std::string *error); + Token Lex(std::string *error); + bool Reduce(std::string *error); + bool Parse(std::string *error, size_t reduceUntil = 0); + + static Expr *Parse(const char *input, std::string *error); +}; + +ExprParser::Token ExprParser::Token::From(TokenType type, Expr *expr) { + Token t; + t.type = type; + t.expr = expr; + return t; +} + +ExprParser::Token ExprParser::Token::From(TokenType type, Expr::Op op) { + Token t; + t.type = type; + t.expr = Expr::AllocExpr(); + t.expr->op = op; + return t; +} + +char ExprParser::ReadChar() { + return input[inputPos++]; +} + +char ExprParser::PeekChar() { + return input[inputPos]; +} + +std::string ExprParser::ReadWord() { + std::string s; + + while(char c = PeekChar()) { + if(!isalnum(c)) break; + s.push_back(ReadChar()); + } + + return s; +} + +void ExprParser::SkipSpace() { + while(char c = PeekChar()) { + if(!isspace(c)) break; + ReadChar(); + } +} + +ExprParser::Token ExprParser::LexNumber(std::string *error) { + std::string s; + + while(char c = PeekChar()) { + if(!((c >= '0' && c <= '9') || c == 'e' || c == 'E' || c == '.' || c == '_')) break; + if(c == '_') { + ReadChar(); + continue; + } + s.push_back(ReadChar()); + } + + char *endptr; + double d = strtod(s.c_str(), &endptr); + + Token t = Token::From(); + if(endptr == s.c_str() + s.size()) { + t = Token::From(TokenType::OPERAND, Expr::Op::CONSTANT); + t.expr->v = d; + } else { + *error = "'" + s + "' is not a valid number"; + } + return t; +} + +ExprParser::Token ExprParser::Lex(std::string *error) { + SkipSpace(); + + Token t = Token::From(); + char c = PeekChar(); + if(isupper(c)) { + std::string n = ReadWord(); + t = Token::From(TokenType::OPERAND, Expr::Op::VARIABLE); + } else if(isalpha(c)) { + std::string s = ReadWord(); + if(s == "sqrt") { + t = Token::From(TokenType::UNARY_OP, Expr::Op::SQRT); + } else if(s == "square") { + t = Token::From(TokenType::UNARY_OP, Expr::Op::SQUARE); + } else if(s == "sin") { + t = Token::From(TokenType::UNARY_OP, Expr::Op::SIN); + } else if(s == "cos") { + t = Token::From(TokenType::UNARY_OP, Expr::Op::COS); + } else if(s == "asin") { + t = Token::From(TokenType::UNARY_OP, Expr::Op::ASIN); + } else if(s == "acos") { + t = Token::From(TokenType::UNARY_OP, Expr::Op::ACOS); + } else if(s == "pi") { + t = Token::From(TokenType::OPERAND, Expr::Op::CONSTANT); + t.expr->v = PI; + } else { + *error = "'" + s + "' is not a valid variable, function or constant"; + } + } else if(isdigit(c) || c == '.') { + return LexNumber(error); + } else if(ispunct(c)) { + ReadChar(); + if(c == '+') { + t = Token::From(TokenType::BINARY_OP, Expr::Op::PLUS); + } else if(c == '-') { + t = Token::From(TokenType::BINARY_OP, Expr::Op::MINUS); + } else if(c == '*') { + t = Token::From(TokenType::BINARY_OP, Expr::Op::TIMES); + } else if(c == '/') { + t = Token::From(TokenType::BINARY_OP, Expr::Op::DIV); + } else if(c == '(') { + t = Token::From(TokenType::PAREN_LEFT); + } else if(c == ')') { + t = Token::From(TokenType::PAREN_RIGHT); + } else { + *error = "'" + std::string(1, c) + "' is not a valid operator"; + } + } else if(c == '\0') { + t = Token::From(TokenType::END); + } else { + *error = "Unexpected character '" + std::string(1, c) + "'"; + } + + return t; +} + +ExprParser::Token ExprParser::PopOperand(std::string *error) { + Token t = Token::From(); + if(stack.empty() || stack.back().type != TokenType::OPERAND) { + *error = "Expected an operand"; + } else { + t = stack.back(); + stack.pop_back(); + } + return t; +} + +ExprParser::Token ExprParser::PopOperator(std::string *error) { + Token t = Token::From(); + if(stack.empty() || (stack.back().type != TokenType::UNARY_OP && + stack.back().type != TokenType::BINARY_OP)) { + *error = "Expected an operator"; + } else { + t = stack.back(); + stack.pop_back(); + } + return t; +} + +int ExprParser::Precedence(Token t) { + ssassert(t.type == TokenType::BINARY_OP || + t.type == TokenType::UNARY_OP || + t.type == TokenType::OPERAND, + "Unexpected token type"); + + if(t.type == TokenType::UNARY_OP) { + return 30; + } else if(t.expr->op == Expr::Op::TIMES || + t.expr->op == Expr::Op::DIV) { + return 20; + } else if(t.expr->op == Expr::Op::PLUS || + t.expr->op == Expr::Op::MINUS) { + return 10; + } else if(t.type == TokenType::OPERAND) { + return 0; + } else ssassert(false, "Unexpected operator"); +} + +bool ExprParser::Reduce(std::string *error) { + Token a = PopOperand(error); + if(a.IsError()) return false; + + Token op = PopOperator(error); + if(op.IsError()) return false; + + Token r = Token::From(TokenType::OPERAND); + switch(op.type) { + case TokenType::BINARY_OP: { + Token b = PopOperand(error); + if(b.IsError()) return false; + r.expr = b.expr->AnyOp(op.expr->op, a.expr); + break; + } + + case TokenType::UNARY_OP: { + Expr *e = a.expr; + switch(op.expr->op) { + case Expr::Op::NEGATE: e = e->Negate(); break; + case Expr::Op::SQRT: e = e->Sqrt(); break; + case Expr::Op::SQUARE: e = e->Times(e); break; + case Expr::Op::SIN: e = e->Times(Expr::From(PI/180))->Sin(); break; + case Expr::Op::COS: e = e->Times(Expr::From(PI/180))->Cos(); break; + case Expr::Op::ASIN: e = e->ASin()->Times(Expr::From(180/PI)); break; + case Expr::Op::ACOS: e = e->ACos()->Times(Expr::From(180/PI)); break; + default: ssassert(false, "Unexpected unary operator"); + } + r.expr = e; + break; + } + + default: ssassert(false, "Unexpected operator"); + } + stack.push_back(r); + + return true; +} + +bool ExprParser::Parse(std::string *error, size_t reduceUntil) { + while(true) { + Token t = Lex(error); + switch(t.type) { + case TokenType::ERROR: + return false; + + case TokenType::END: + case TokenType::PAREN_RIGHT: + while(stack.size() > 1 + reduceUntil) { + if(!Reduce(error)) return false; + } + + if(t.type == TokenType::PAREN_RIGHT) { + stack.push_back(t); + } + return true; + + case TokenType::PAREN_LEFT: { + // sub-expression + if(!Parse(error, /*reduceUntil=*/stack.size())) return false; + + if(stack.empty() || stack.back().type != TokenType::PAREN_RIGHT) { + *error = "Expected ')'"; + return false; + } + stack.pop_back(); + break; + } + + case TokenType::BINARY_OP: + if((stack.size() > reduceUntil && stack.back().type != TokenType::OPERAND) || + stack.size() == reduceUntil) { + if(t.expr->op == Expr::Op::MINUS) { + t.type = TokenType::UNARY_OP; + t.expr->op = Expr::Op::NEGATE; + stack.push_back(t); + break; + } + } + + while(stack.size() > 1 + reduceUntil && + Precedence(t) <= Precedence(stack[stack.size() - 2])) { + if(!Reduce(error)) return false; + } + + stack.push_back(t); + break; + + case TokenType::UNARY_OP: + case TokenType::OPERAND: + stack.push_back(t); + break; + } + } + + return true; +} + +Expr *ExprParser::Parse(const char *input, std::string *error) { + ExprParser parser; + parser.input = input; + parser.inputPos = 0; + if(!parser.Parse(error)) return NULL; + + Token r = parser.PopOperand(error); + if(r.IsError()) return NULL; + return r.expr; +} + +Expr *Expr::Parse(const char *input, std::string *error) { + return ExprParser::Parse(input, error); +} + +Expr *Expr::From(const char *input, bool popUpError) { + std::string error; + Expr *e = ExprParser::Parse(input, &error); + if(!e) { + dbp("Parse/lex error: %s", error.c_str()); + if(popUpError) { + Error("Not a valid number or expression: '%s'.\n%s.", input, error.c_str()); + } + } + return e; +} diff --git a/ThirdParty/libslvs/expr.h b/ThirdParty/libslvs/expr.h new file mode 100644 index 0000000000..ea11310537 --- /dev/null +++ b/ThirdParty/libslvs/expr.h @@ -0,0 +1,140 @@ +//----------------------------------------------------------------------------- +// An expression in our symbolic algebra system, used to write, linearize, +// and solve our constraint equations. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- +#ifndef __EXPR_H +#define __EXPR_H + +class Expr { +public: + + enum class Op : uint32_t { + // A parameter, by the hParam handle + PARAM = 0, + // A parameter, by a pointer straight in to the param table (faster, + // if we know that the param table won't move around) + PARAM_PTR = 1, + + // Operands + CONSTANT = 20, + VARIABLE = 21, + + // Binary ops + PLUS = 100, + MINUS = 101, + TIMES = 102, + DIV = 103, + // Unary ops + NEGATE = 104, + SQRT = 105, + SQUARE = 106, + SIN = 107, + COS = 108, + ASIN = 109, + ACOS = 110, + }; + + Op op; + Expr *a; + union { + double v; + hParam parh; + Param *parp; + Expr *b; + }; + + Expr() { } + Expr(double val) : op(Op::CONSTANT) { v = val; } + + static inline Expr *AllocExpr() + { return (Expr *)AllocTemporary(sizeof(Expr)); } + + static Expr *From(hParam p); + static Expr *From(double v); + + Expr *AnyOp(Op op, Expr *b); + inline Expr *Plus (Expr *b_) { return AnyOp(Op::PLUS, b_); } + inline Expr *Minus(Expr *b_) { return AnyOp(Op::MINUS, b_); } + inline Expr *Times(Expr *b_) { return AnyOp(Op::TIMES, b_); } + inline Expr *Div (Expr *b_) { return AnyOp(Op::DIV, b_); } + + inline Expr *Negate() { return AnyOp(Op::NEGATE, NULL); } + inline Expr *Sqrt () { return AnyOp(Op::SQRT, NULL); } + inline Expr *Square() { return AnyOp(Op::SQUARE, NULL); } + inline Expr *Sin () { return AnyOp(Op::SIN, NULL); } + inline Expr *Cos () { return AnyOp(Op::COS, NULL); } + inline Expr *ASin () { return AnyOp(Op::ASIN, NULL); } + inline Expr *ACos () { return AnyOp(Op::ACOS, NULL); } + + Expr *PartialWrt(hParam p) const; + double Eval() const; + uint64_t ParamsUsed() const; + bool DependsOn(hParam p) const; + static bool Tol(double a, double b); + Expr *FoldConstants(); + void Substitute(hParam oldh, hParam newh); + + static const hParam NO_PARAMS, MULTIPLE_PARAMS; + hParam ReferencedParams(ParamList *pl) const; + + void ParamsToPointers(); + + std::string Print() const; + + // number of child nodes: 0 (e.g. constant), 1 (sqrt), or 2 (+) + int Children() const; + // total number of nodes in the tree + int Nodes() const; + + // Make a simple copy + Expr *DeepCopy() const; + // Make a copy, with the parameters (usually referenced by hParam) + // resolved to pointers to the actual value. This speeds things up + // considerably. + Expr *DeepCopyWithParamsAsPointers(IdList *firstTry, + IdList *thenTry) const; + + static Expr *Parse(const char *input, std::string *error); + static Expr *From(const char *in, bool popUpError); +}; + +class ExprVector { +public: + Expr *x, *y, *z; + + static ExprVector From(Expr *x, Expr *y, Expr *z); + static ExprVector From(Vector vn); + static ExprVector From(hParam x, hParam y, hParam z); + static ExprVector From(double x, double y, double z); + + ExprVector Plus(ExprVector b) const; + ExprVector Minus(ExprVector b) const; + Expr *Dot(ExprVector b) const; + ExprVector Cross(ExprVector b) const; + ExprVector ScaledBy(Expr *s) const; + ExprVector WithMagnitude(Expr *s) const; + Expr *Magnitude() const; + + Vector Eval() const; +}; + +class ExprQuaternion { +public: + Expr *w, *vx, *vy, *vz; + + static ExprQuaternion From(Expr *w, Expr *vx, Expr *vy, Expr *vz); + static ExprQuaternion From(Quaternion qn); + static ExprQuaternion From(hParam w, hParam vx, hParam vy, hParam vz); + + ExprVector RotationU() const; + ExprVector RotationV() const; + ExprVector RotationN() const; + + ExprVector Rotate(ExprVector p) const; + ExprQuaternion Times(ExprQuaternion b) const; + + Expr *Magnitude() const; +}; +#endif diff --git a/ThirdParty/libslvs/include/SolveSpaceSystem.h b/ThirdParty/libslvs/include/SolveSpaceSystem.h new file mode 100644 index 0000000000..0e31fba5fc --- /dev/null +++ b/ThirdParty/libslvs/include/SolveSpaceSystem.h @@ -0,0 +1,51 @@ +#pragma once + +#include "slvs.h" + +#include +#include +#include + +class DLL SolveSpaceSystem +{ +public: + SolveSpaceSystem(); + + Slvs_hParam addParam(Slvs_Param parameter); + + Slvs_hEntity addEntity(Slvs_Entity entity); + + Slvs_hConstraint addConstr(Slvs_Constraint constr); + + enum ResultStatus { + RESULT_OKAY = SLVS_RESULT_OKAY , + RESULT_INCONSISTENT = SLVS_RESULT_INCONSISTENT , + RESULT_DIDNT_CONVERGE = SLVS_RESULT_DIDNT_CONVERGE , + RESULT_TOO_MANY_UNKNOWNS = SLVS_RESULT_TOO_MANY_UNKNOWNS, + }; + + ResultStatus solve(Slvs_hGroup groupId, bool reportFailedConstraints = true); + + double parameterValue(Slvs_hParam paramId); + + std::tuple< std::valarray, + std::valarray, + std::valarray > + orientationMx(Slvs_hEntity normalIn3dEntityId); + + // Returns point as x, y, z values + std::valarray global3DPos (Slvs_hEntity pointEntityId); + + Slvs_Constraint constraint(Slvs_hConstraint constraintId); + std::vector failedConstraints() const; + +private: + Slvs_System m_slvsSystem; + + std::vector * m_paramsMemory; + std::vector * m_entityMemory; + std::vector * m_constraintMemory; + std::vector* m_failedConstrMemory; +}; + + diff --git a/ThirdParty/libslvs/include/slvs.h b/ThirdParty/libslvs/include/slvs.h new file mode 100644 index 0000000000..4fd07681de --- /dev/null +++ b/ThirdParty/libslvs/include/slvs.h @@ -0,0 +1,409 @@ +/*----------------------------------------------------------------------------- + * Data structures and prototypes for slvs.lib, a geometric constraint solver. + * + * See the comments in this file, the accompanying sample code that uses + * this library, and the accompanying documentation (DOC.txt). + * + * Copyright 2009-2013 Jonathan Westhues. + *---------------------------------------------------------------------------*/ + +#ifndef __SLVS_H +#define __SLVS_H + +#ifdef SLVS_LIB_SHARED +#ifdef WIN32 +# ifdef EXPORT_DLL +# define DLL __declspec( dllexport ) +# else +# define DLL __declspec( dllimport ) +# endif +#else +# define DLL +#endif +#else +# define DLL +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef _MSC_VER +typedef unsigned __int32 uint32_t; +#else +#include +#endif +#include + +typedef uint32_t Slvs_hParam; +typedef uint32_t Slvs_hEntity; +typedef uint32_t Slvs_hConstraint; +typedef uint32_t Slvs_hGroup; + +/* To obtain the 3d (not projected into a workplane) of a constraint or + * an entity, specify this instead of the workplane. */ +#define SLVS_FREE_IN_3D 0 + + +typedef struct { + Slvs_hParam h; + Slvs_hGroup group; + double val; +} Slvs_Param; + + +#define SLVS_E_POINT_IN_3D 50000 +#define SLVS_E_POINT_IN_2D 50001 + +#define SLVS_E_NORMAL_IN_3D 60000 +#define SLVS_E_NORMAL_IN_2D 60001 + +#define SLVS_E_DISTANCE 70000 + +/* The special point, normal, and distance types used for parametric step + * and repeat, extrude, and assembly are currently not exposed. Please + * contact us if you are interested in using these. */ + +#define SLVS_E_WORKPLANE 80000 +#define SLVS_E_LINE_SEGMENT 80001 +#define SLVS_E_CUBIC 80002 +#define SLVS_E_CIRCLE 80003 +#define SLVS_E_ARC_OF_CIRCLE 80004 + +typedef struct { + Slvs_hEntity h; + Slvs_hGroup group; + + int type; + + Slvs_hEntity wrkpl; + Slvs_hEntity point[4]; + Slvs_hEntity normal; + Slvs_hEntity distance; + + Slvs_hParam param[4]; +} Slvs_Entity; + +#define SLVS_C_POINTS_COINCIDENT 100000 +#define SLVS_C_PT_PT_DISTANCE 100001 +#define SLVS_C_PT_PLANE_DISTANCE 100002 +#define SLVS_C_PT_LINE_DISTANCE 100003 +#define SLVS_C_PT_FACE_DISTANCE 100004 +#define SLVS_C_PT_IN_PLANE 100005 +#define SLVS_C_PT_ON_LINE 100006 +#define SLVS_C_PT_ON_FACE 100007 +#define SLVS_C_EQUAL_LENGTH_LINES 100008 +#define SLVS_C_LENGTH_RATIO 100009 +#define SLVS_C_EQ_LEN_PT_LINE_D 100010 +#define SLVS_C_EQ_PT_LN_DISTANCES 100011 +#define SLVS_C_EQUAL_ANGLE 100012 +#define SLVS_C_EQUAL_LINE_ARC_LEN 100013 +#define SLVS_C_SYMMETRIC 100014 +#define SLVS_C_SYMMETRIC_HORIZ 100015 +#define SLVS_C_SYMMETRIC_VERT 100016 +#define SLVS_C_SYMMETRIC_LINE 100017 +#define SLVS_C_AT_MIDPOINT 100018 +#define SLVS_C_HORIZONTAL 100019 +#define SLVS_C_VERTICAL 100020 +#define SLVS_C_DIAMETER 100021 +#define SLVS_C_PT_ON_CIRCLE 100022 +#define SLVS_C_SAME_ORIENTATION 100023 +#define SLVS_C_ANGLE 100024 +#define SLVS_C_PARALLEL 100025 +#define SLVS_C_PERPENDICULAR 100026 +#define SLVS_C_ARC_LINE_TANGENT 100027 +#define SLVS_C_CUBIC_LINE_TANGENT 100028 +#define SLVS_C_EQUAL_RADIUS 100029 +#define SLVS_C_PROJ_PT_DISTANCE 100030 +#define SLVS_C_WHERE_DRAGGED 100031 +#define SLVS_C_CURVE_CURVE_TANGENT 100032 +#define SLVS_C_LENGTH_DIFFERENCE 100033 + +typedef struct { + Slvs_hConstraint h; + Slvs_hGroup group; + + int type; + + Slvs_hEntity wrkpl; + + double valA; + Slvs_hEntity ptA; + Slvs_hEntity ptB; + Slvs_hEntity entityA; + Slvs_hEntity entityB; + Slvs_hEntity entityC; + Slvs_hEntity entityD; + + int other; + int other2; +} Slvs_Constraint; + + +typedef struct { + /*** INPUT VARIABLES + * + * Here, we specify the parameters and their initial values, the entities, + * and the constraints. For example, param[] points to the array of + * parameters, which has length params, so that the last valid element + * is param[params-1]. + * + * param[] is actually an in/out variable; if the solver is successful, + * then the new values (that satisfy the constraints) are written to it. */ + Slvs_Param *param; + int params; + Slvs_Entity *entity; + int entities; + Slvs_Constraint *constraint; + int constraints; + + /* If a parameter corresponds to a point (distance, normal, etc.) being + * dragged, then specify it here. This will cause the solver to favor + * that parameter, and attempt to change it as little as possible even + * if that requires it to change other parameters more. + * + * Unused members of this array should be set to zero. */ + Slvs_hParam dragged[4]; + + /* If the solver fails, then it can determine which constraints are + * causing the problem. But this is a relatively slow process (for + * a system with n constraints, about n times as long as just solving). + * If calculateFaileds is true, then the solver will do so, otherwise + * not. */ + int calculateFaileds; + + /*** OUTPUT VARIABLES + * + * If the solver fails, then it can report which constraints are causing + * the problem. The caller should allocate the array failed[], and pass + * its size in faileds. + * + * The solver will set faileds equal to the number of problematic + * constraints, and write their Slvs_hConstraints into failed[]. To + * ensure that there is sufficient space for any possible set of + * failing constraints, faileds should be greater than or equal to + * constraints. */ + Slvs_hConstraint *failed; + int faileds; + + /* The solver indicates the number of unconstrained degrees of freedom. */ + int dof; + + /* The solver indicates whether the solution succeeded. */ +#define SLVS_RESULT_OKAY 0 +#define SLVS_RESULT_INCONSISTENT 1 +#define SLVS_RESULT_DIDNT_CONVERGE 2 +#define SLVS_RESULT_TOO_MANY_UNKNOWNS 3 + int result; +} Slvs_System; + +DLL void Slvs_Solve(Slvs_System *sys, Slvs_hGroup hg); + + +/* Our base coordinate system has basis vectors + * (1, 0, 0) (0, 1, 0) (0, 0, 1) + * A unit quaternion defines a rotation to a new coordinate system with + * basis vectors + * U V N + * which these functions compute from the quaternion. */ +DLL void Slvs_QuaternionU(double qw, double qx, double qy, double qz, + double *x, double *y, double *z); +DLL void Slvs_QuaternionV(double qw, double qx, double qy, double qz, + double *x, double *y, double *z); +DLL void Slvs_QuaternionN(double qw, double qx, double qy, double qz, + double *x, double *y, double *z); + +/* Similarly, compute a unit quaternion in terms of two basis vectors. */ +DLL void Slvs_MakeQuaternion(double ux, double uy, double uz, + double vx, double vy, double vz, + double *qw, double *qx, double *qy, double *qz); + + +/*------------------------------------- + * These are just convenience functions, to save you the trouble of filling + * out the structures by hand. The code is included in the header file to + * let the compiler inline them if possible. */ + +static inline Slvs_Param Slvs_MakeParam(Slvs_hParam h, Slvs_hGroup group, double val) +{ + Slvs_Param r; + r.h = h; + r.group = group; + r.val = val; + return r; +} +static inline Slvs_Entity Slvs_MakePoint2d(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hEntity wrkpl, + Slvs_hParam u, Slvs_hParam v) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_POINT_IN_2D; + r.wrkpl = wrkpl; + r.param[0] = u; + r.param[1] = v; + return r; +} +static inline Slvs_Entity Slvs_MakePoint3d(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hParam x, Slvs_hParam y, Slvs_hParam z) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_POINT_IN_3D; + r.wrkpl = SLVS_FREE_IN_3D; + r.param[0] = x; + r.param[1] = y; + r.param[2] = z; + return r; +} +static inline Slvs_Entity Slvs_MakeNormal3d(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hParam qw, Slvs_hParam qx, + Slvs_hParam qy, Slvs_hParam qz) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_NORMAL_IN_3D; + r.wrkpl = SLVS_FREE_IN_3D; + r.param[0] = qw; + r.param[1] = qx; + r.param[2] = qy; + r.param[3] = qz; + return r; +} +static inline Slvs_Entity Slvs_MakeNormal2d(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hEntity wrkpl) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_NORMAL_IN_2D; + r.wrkpl = wrkpl; + return r; +} +static inline Slvs_Entity Slvs_MakeDistance(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hEntity wrkpl, Slvs_hParam d) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_DISTANCE; + r.wrkpl = wrkpl; + r.param[0] = d; + return r; +} +static inline Slvs_Entity Slvs_MakeLineSegment(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hEntity wrkpl, + Slvs_hEntity ptA, Slvs_hEntity ptB) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_LINE_SEGMENT; + r.wrkpl = wrkpl; + r.point[0] = ptA; + r.point[1] = ptB; + return r; +} +static inline Slvs_Entity Slvs_MakeCubic(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hEntity wrkpl, + Slvs_hEntity pt0, Slvs_hEntity pt1, + Slvs_hEntity pt2, Slvs_hEntity pt3) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_CUBIC; + r.wrkpl = wrkpl; + r.point[0] = pt0; + r.point[1] = pt1; + r.point[2] = pt2; + r.point[3] = pt3; + return r; +} +static inline Slvs_Entity Slvs_MakeArcOfCircle(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hEntity wrkpl, + Slvs_hEntity normal, + Slvs_hEntity center, + Slvs_hEntity start, Slvs_hEntity end) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_ARC_OF_CIRCLE; + r.wrkpl = wrkpl; + r.normal = normal; + r.point[0] = center; + r.point[1] = start; + r.point[2] = end; + return r; +} +static inline Slvs_Entity Slvs_MakeCircle(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hEntity wrkpl, + Slvs_hEntity center, + Slvs_hEntity normal, Slvs_hEntity radius) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_CIRCLE; + r.wrkpl = wrkpl; + r.point[0] = center; + r.normal = normal; + r.distance = radius; + return r; +} +static inline Slvs_Entity Slvs_MakeWorkplane(Slvs_hEntity h, Slvs_hGroup group, + Slvs_hEntity origin, Slvs_hEntity normal) +{ + Slvs_Entity r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = SLVS_E_WORKPLANE; + r.wrkpl = SLVS_FREE_IN_3D; + r.point[0] = origin; + r.normal = normal; + return r; +} + +static inline Slvs_Constraint Slvs_MakeConstraint(Slvs_hConstraint h, + Slvs_hGroup group, + int type, + Slvs_hEntity wrkpl, + double valA, + Slvs_hEntity ptA, + Slvs_hEntity ptB, + Slvs_hEntity entityA, + Slvs_hEntity entityB) +{ + Slvs_Constraint r; + memset(&r, 0, sizeof(r)); + r.h = h; + r.group = group; + r.type = type; + r.wrkpl = wrkpl; + r.valA = valA; + r.ptA = ptA; + r.ptB = ptB; + r.entityA = entityA; + r.entityB = entityB; + return r; +} + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/ThirdParty/libslvs/lib.cpp b/ThirdParty/libslvs/lib.cpp new file mode 100644 index 0000000000..4e5e7fdf81 --- /dev/null +++ b/ThirdParty/libslvs/lib.cpp @@ -0,0 +1,272 @@ +//----------------------------------------------------------------------------- +// A library wrapper around SolveSpace, to permit someone to use its constraint +// solver without coupling their program too much to SolveSpace's internals. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- +#include "solvespace.h" +#define EXPORT_DLL +#include + +Sketch SolveSpace::SK = {}; +static System SYS; + +static int IsInit = 0; + +void Group::GenerateEquations(IdList *) { + // Nothing to do for now. +} + +void SolveSpace::CnfFreezeInt(uint32_t, const std::string &) +{ + abort(); +} + +uint32_t SolveSpace::CnfThawInt(uint32_t, const std::string &) +{ + abort(); + return 0; +} + +void SolveSpace::DoMessageBox(const char *, int, int, bool) +{ + abort(); +} + +extern "C" { + +void Slvs_QuaternionU(double qw, double qx, double qy, double qz, + double *x, double *y, double *z) +{ + Quaternion q = Quaternion::From(qw, qx, qy, qz); + Vector v = q.RotationU(); + *x = v.x; + *y = v.y; + *z = v.z; +} + +void Slvs_QuaternionV(double qw, double qx, double qy, double qz, + double *x, double *y, double *z) +{ + Quaternion q = Quaternion::From(qw, qx, qy, qz); + Vector v = q.RotationV(); + *x = v.x; + *y = v.y; + *z = v.z; +} + +void Slvs_QuaternionN(double qw, double qx, double qy, double qz, + double *x, double *y, double *z) +{ + Quaternion q = Quaternion::From(qw, qx, qy, qz); + Vector v = q.RotationN(); + *x = v.x; + *y = v.y; + *z = v.z; +} + +void Slvs_MakeQuaternion(double ux, double uy, double uz, + double vx, double vy, double vz, + double *qw, double *qx, double *qy, double *qz) +{ + Vector u = Vector::From(ux, uy, uz), + v = Vector::From(vx, vy, vz); + Quaternion q = Quaternion::From(u, v); + *qw = q.w; + *qx = q.vx; + *qy = q.vy; + *qz = q.vz; +} + +void Slvs_Solve(Slvs_System *ssys, Slvs_hGroup shg) +{ + if(!IsInit) { + InitPlatform(0, NULL); + IsInit = 1; + } + + int i; + for(i = 0; i < ssys->params; i++) { + Slvs_Param *sp = &(ssys->param[i]); + Param p = {}; + + p.h.v = sp->h; + p.val = sp->val; + SK.param.Add(&p); + if(sp->group == shg) { + SYS.param.Add(&p); + } + } + + for(i = 0; i < ssys->entities; i++) { + Slvs_Entity *se = &(ssys->entity[i]); + EntityBase e = {}; + + switch(se->type) { +case SLVS_E_POINT_IN_3D: e.type = Entity::Type::POINT_IN_3D; break; +case SLVS_E_POINT_IN_2D: e.type = Entity::Type::POINT_IN_2D; break; +case SLVS_E_NORMAL_IN_3D: e.type = Entity::Type::NORMAL_IN_3D; break; +case SLVS_E_NORMAL_IN_2D: e.type = Entity::Type::NORMAL_IN_2D; break; +case SLVS_E_DISTANCE: e.type = Entity::Type::DISTANCE; break; +case SLVS_E_WORKPLANE: e.type = Entity::Type::WORKPLANE; break; +case SLVS_E_LINE_SEGMENT: e.type = Entity::Type::LINE_SEGMENT; break; +case SLVS_E_CUBIC: e.type = Entity::Type::CUBIC; break; +case SLVS_E_CIRCLE: e.type = Entity::Type::CIRCLE; break; +case SLVS_E_ARC_OF_CIRCLE: e.type = Entity::Type::ARC_OF_CIRCLE; break; + +default: dbp("bad entity type %d", se->type); return; + } + e.h.v = se->h; + e.group.v = se->group; + e.workplane.v = se->wrkpl; + e.point[0].v = se->point[0]; + e.point[1].v = se->point[1]; + e.point[2].v = se->point[2]; + e.point[3].v = se->point[3]; + e.normal.v = se->normal; + e.distance.v = se->distance; + e.param[0].v = se->param[0]; + e.param[1].v = se->param[1]; + e.param[2].v = se->param[2]; + e.param[3].v = se->param[3]; + + SK.entity.Add(&e); + } + IdList params = {}; + for(i = 0; i < ssys->constraints; i++) { + Slvs_Constraint *sc = &(ssys->constraint[i]); + ConstraintBase c = {}; + + Constraint::Type t; + switch(sc->type) { +case SLVS_C_POINTS_COINCIDENT: t = Constraint::Type::POINTS_COINCIDENT; break; +case SLVS_C_PT_PT_DISTANCE: t = Constraint::Type::PT_PT_DISTANCE; break; +case SLVS_C_PT_PLANE_DISTANCE: t = Constraint::Type::PT_PLANE_DISTANCE; break; +case SLVS_C_PT_LINE_DISTANCE: t = Constraint::Type::PT_LINE_DISTANCE; break; +case SLVS_C_PT_FACE_DISTANCE: t = Constraint::Type::PT_FACE_DISTANCE; break; +case SLVS_C_PT_IN_PLANE: t = Constraint::Type::PT_IN_PLANE; break; +case SLVS_C_PT_ON_LINE: t = Constraint::Type::PT_ON_LINE; break; +case SLVS_C_PT_ON_FACE: t = Constraint::Type::PT_ON_FACE; break; +case SLVS_C_EQUAL_LENGTH_LINES: t = Constraint::Type::EQUAL_LENGTH_LINES; break; +case SLVS_C_LENGTH_RATIO: t = Constraint::Type::LENGTH_RATIO; break; +case SLVS_C_EQ_LEN_PT_LINE_D: t = Constraint::Type::EQ_LEN_PT_LINE_D; break; +case SLVS_C_EQ_PT_LN_DISTANCES: t = Constraint::Type::EQ_PT_LN_DISTANCES; break; +case SLVS_C_EQUAL_ANGLE: t = Constraint::Type::EQUAL_ANGLE; break; +case SLVS_C_EQUAL_LINE_ARC_LEN: t = Constraint::Type::EQUAL_LINE_ARC_LEN; break; +case SLVS_C_LENGTH_DIFFERENCE: t = Constraint::Type::LENGTH_DIFFERENCE; break; +case SLVS_C_SYMMETRIC: t = Constraint::Type::SYMMETRIC; break; +case SLVS_C_SYMMETRIC_HORIZ: t = Constraint::Type::SYMMETRIC_HORIZ; break; +case SLVS_C_SYMMETRIC_VERT: t = Constraint::Type::SYMMETRIC_VERT; break; +case SLVS_C_SYMMETRIC_LINE: t = Constraint::Type::SYMMETRIC_LINE; break; +case SLVS_C_AT_MIDPOINT: t = Constraint::Type::AT_MIDPOINT; break; +case SLVS_C_HORIZONTAL: t = Constraint::Type::HORIZONTAL; break; +case SLVS_C_VERTICAL: t = Constraint::Type::VERTICAL; break; +case SLVS_C_DIAMETER: t = Constraint::Type::DIAMETER; break; +case SLVS_C_PT_ON_CIRCLE: t = Constraint::Type::PT_ON_CIRCLE; break; +case SLVS_C_SAME_ORIENTATION: t = Constraint::Type::SAME_ORIENTATION; break; +case SLVS_C_ANGLE: t = Constraint::Type::ANGLE; break; +case SLVS_C_PARALLEL: t = Constraint::Type::PARALLEL; break; +case SLVS_C_PERPENDICULAR: t = Constraint::Type::PERPENDICULAR; break; +case SLVS_C_ARC_LINE_TANGENT: t = Constraint::Type::ARC_LINE_TANGENT; break; +case SLVS_C_CUBIC_LINE_TANGENT: t = Constraint::Type::CUBIC_LINE_TANGENT; break; +case SLVS_C_EQUAL_RADIUS: t = Constraint::Type::EQUAL_RADIUS; break; +case SLVS_C_PROJ_PT_DISTANCE: t = Constraint::Type::PROJ_PT_DISTANCE; break; +case SLVS_C_WHERE_DRAGGED: t = Constraint::Type::WHERE_DRAGGED; break; +case SLVS_C_CURVE_CURVE_TANGENT:t = Constraint::Type::CURVE_CURVE_TANGENT; break; + +default: dbp("bad constraint type %d", sc->type); return; + } + + c.type = t; + + c.h.v = sc->h; + c.group.v = sc->group; + c.workplane.v = sc->wrkpl; + c.valA = sc->valA; + c.ptA.v = sc->ptA; + c.ptB.v = sc->ptB; + c.entityA.v = sc->entityA; + c.entityB.v = sc->entityB; + c.entityC.v = sc->entityC; + c.entityD.v = sc->entityD; + c.other = (sc->other) ? true : false; + c.other2 = (sc->other2) ? true : false; + + c.Generate(¶ms); + if(params.n > 0) { + for(Param &p : params) { + p.h = SK.param.AddAndAssignId(&p); + c.valP = p.h; + SYS.param.Add(&p); + } + params.Clear(); + c.ModifyToSatisfy(); + } + + SK.constraint.Add(&c); + } + + for(i = 0; i < (int)arraylen(ssys->dragged); i++) { + if(ssys->dragged[i]) { + hParam hp = { ssys->dragged[i] }; + SYS.dragged.Add(&hp); + } + } + + Group g = {}; + g.h.v = shg; + + List bad = {}; + + // Now we're finally ready to solve! + bool andFindBad = ssys->calculateFaileds ? true : false; + SolveResult how = SYS.Solve(&g, &(ssys->dof), &bad, andFindBad, /*andFindFree=*/false); + + switch(how) { + case SolveResult::OKAY: + ssys->result = SLVS_RESULT_OKAY; + break; + + case SolveResult::DIDNT_CONVERGE: + ssys->result = SLVS_RESULT_DIDNT_CONVERGE; + break; + + case SolveResult::REDUNDANT_DIDNT_CONVERGE: + case SolveResult::REDUNDANT_OKAY: + ssys->result = SLVS_RESULT_INCONSISTENT; + break; + + case SolveResult::TOO_MANY_UNKNOWNS: + ssys->result = SLVS_RESULT_TOO_MANY_UNKNOWNS; + break; + } + + // Write the new parameter values back to our caller. + for(i = 0; i < ssys->params; i++) { + Slvs_Param *sp = &(ssys->param[i]); + hParam hp = { sp->h }; + sp->val = SK.GetParam(hp)->val; + } + + if(ssys->failed) { + // Copy over any the list of problematic constraints. + for(i = 0; i < ssys->faileds && i < bad.n; i++) { + ssys->failed[i] = bad.elem[i].v; + } + ssys->faileds = bad.n; + } + + bad.Clear(); + SYS.param.Clear(); + SYS.entity.Clear(); + SYS.eq.Clear(); + SYS.dragged.Clear(); + + SK.param.Clear(); + SK.entity.Clear(); + SK.constraint.Clear(); + + FreeAllTemporary(); +} + +} /* extern "C" */ diff --git a/ThirdParty/libslvs/platform/platform.h b/ThirdParty/libslvs/platform/platform.h new file mode 100644 index 0000000000..06ae19ffc4 --- /dev/null +++ b/ThirdParty/libslvs/platform/platform.h @@ -0,0 +1,68 @@ +//----------------------------------------------------------------------------- +// Platform-dependent functionality. +// +// Copyright 2017 whitequark +//----------------------------------------------------------------------------- + +#ifndef SOLVESPACE_PLATFORM_H +#define SOLVESPACE_PLATFORM_H + +namespace Platform { + +// UTF-8 ⟷ UTF-16 conversion, for Windows. +#if defined(WIN32) +std::string Narrow(const wchar_t *s); +std::wstring Widen(const char *s); +std::string Narrow(const std::wstring &s); +std::wstring Widen(const std::string &s); +#endif + +// A filesystem path, respecting the conventions of the current platform. +// Transformation functions return an empty path on error. +class Path { +public: + std::string raw; + + static Path From(std::string raw); + static Path CurrentDirectory(); + + void Clear() { raw.clear(); } + + bool Equals(const Path &other) const; + bool IsEmpty() const { return raw.empty(); } + bool IsAbsolute() const; + bool HasExtension(std::string ext) const; + + std::string FileName() const; + std::string FileStem() const; + std::string Extension() const; + + Path WithExtension(std::string ext) const; + Path Parent() const; + Path Join(const std::string &component) const; + Path Join(const Path &other) const; + Path Expand(bool fromCurrentDirectory = false) const; + Path RelativeTo(const Path &base) const; + + // Converting to and from a platform-independent representation + // (conventionally, the Unix one). + static Path FromPortable(const std::string &repr); + std::string ToPortable() const; +}; + +struct PathLess { + bool operator()(const Path &a, const Path &b) const { return a.raw < b.raw; } +}; + +// File manipulation functions. +FILE *OpenFile(const Platform::Path &filename, const char *mode); +bool ReadFile(const Platform::Path &filename, std::string *data); +bool WriteFile(const Platform::Path &filename, const std::string &data); +void RemoveFile(const Platform::Path &filename); + +// Resource loading function. +const void *LoadResource(const std::string &name, size_t *size); + +} + +#endif diff --git a/ThirdParty/libslvs/platform/unixutil.cpp b/ThirdParty/libslvs/platform/unixutil.cpp new file mode 100644 index 0000000000..3c8f62d0f9 --- /dev/null +++ b/ThirdParty/libslvs/platform/unixutil.cpp @@ -0,0 +1,124 @@ +//----------------------------------------------------------------------------- +// Utility functions used by the Unix port. Notably, our memory allocation; +// we use two separate allocators, one for long-lived stuff and one for +// stuff that gets freed after every regeneration of the model, to save us +// the trouble of freeing the latter explicitly. +// +// Copyright 2008-2013 Jonathan Westhues. +// Copyright 2013 Daniel Richard G. +//----------------------------------------------------------------------------- +#ifndef LIBRARY +#include +#endif + +#include "solvespace.h" + +namespace SolveSpace { + +void dbp(const char *str, ...) +{ + va_list f; + static char buf[1024*50]; + va_start(f, str); + vsnprintf(buf, sizeof(buf), str, f); + va_end(f); + + fputs(buf, stderr); + fputc('\n', stderr); +} + +void assert_failure(const char *file, unsigned line, const char *function, + const char *condition, const char *message) { + fprintf(stderr, "File %s, line %u, function %s:\n", file, line, function); + fprintf(stderr, "Assertion '%s' failed: ((%s) == false).\n", message, condition); + +#ifndef LIBRARY + static void *ptrs[1024] = {}; + size_t nptrs = backtrace(ptrs, sizeof(ptrs) / sizeof(ptrs[0])); + char **syms = backtrace_symbols(ptrs, nptrs); + + fprintf(stderr, "Backtrace:\n"); + if(syms != NULL) { + for(size_t i = 0; i < nptrs; i++) { + fprintf(stderr, "%2zu: %s\n", i, syms[i]); + } + } else { + for(size_t i = 0; i < nptrs; i++) { + fprintf(stderr, "%2zu: %p\n", i, ptrs[i]); + } + } +#endif + + abort(); +} + +//----------------------------------------------------------------------------- +// A separate heap, on which we allocate expressions. Maybe a bit faster, +// since fragmentation is less of a concern, and it also makes it possible +// to be sloppy with our memory management, and just free everything at once +// at the end. +//----------------------------------------------------------------------------- + +typedef struct _AllocTempHeader AllocTempHeader; + +typedef struct _AllocTempHeader { + AllocTempHeader *prev; + AllocTempHeader *next; +} AllocTempHeader; + +static AllocTempHeader *Head = NULL; + +void *AllocTemporary(size_t n) +{ + AllocTempHeader *h = + (AllocTempHeader *)malloc(n + sizeof(AllocTempHeader)); + h->prev = NULL; + h->next = Head; + if(Head) Head->prev = h; + Head = h; + memset(&h[1], 0, n); + return (void *)&h[1]; +} + +void FreeTemporary(void *p) +{ + AllocTempHeader *h = (AllocTempHeader *)p - 1; + if(h->prev) { + h->prev->next = h->next; + } else { + Head = h->next; + } + if(h->next) h->next->prev = h->prev; + free(h); +} + +void FreeAllTemporary(void) +{ + AllocTempHeader *h = Head; + while(h) { + AllocTempHeader *f = h; + h = h->next; + free(f); + } + Head = NULL; +} + +void *MemAlloc(size_t n) { + void *p = malloc(n); + ssassert(p != NULL, "Cannot allocate memory"); + return p; +} + +void MemFree(void *p) { + free(p); +} + +std::vector InitPlatform(int argc, char **argv) { + std::vector args; + for(int i = 0; i < argc; i++) { + args.push_back(argv[i]); + } + return args; +} + +}; diff --git a/ThirdParty/libslvs/polygon.h b/ThirdParty/libslvs/polygon.h new file mode 100644 index 0000000000..5767dc0359 --- /dev/null +++ b/ThirdParty/libslvs/polygon.h @@ -0,0 +1,415 @@ +//----------------------------------------------------------------------------- +// Anything relating to plane polygons and triangles, and (generally, non- +// planar) meshes thereof. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- + +#ifndef __POLYGON_H +#define __POLYGON_H + +class SPointList; +class SPolygon; +class SContour; +class SMesh; +class SBsp3; +class SOutlineList; + +enum class EarType : uint32_t { + UNKNOWN = 0, + NOT_EAR = 1, + EAR = 2 +}; + +enum class BspClass : uint32_t { + POS = 100, + NEG = 101, + COPLANAR = 200 +}; + +enum class EdgeKind : uint32_t { + NAKED_OR_SELF_INTER = 100, + SELF_INTER = 200, + TURNING = 300, + EMPHASIZED = 400, + SHARP = 500, +}; + +class SEdge { +public: + int tag; + int auxA, auxB; + Vector a, b; + + static SEdge From(Vector a, Vector b); + bool EdgeCrosses(Vector a, Vector b, Vector *pi=NULL, SPointList *spl=NULL) const; +}; + +class SEdgeList { +public: + List l; + + void Clear(); + void AddEdge(Vector a, Vector b, int auxA=0, int auxB=0, int tag=0); + bool AssemblePolygon(SPolygon *dest, SEdge *errorAt, bool keepDir=false) const; + bool AssembleContour(Vector first, Vector last, SContour *dest, + SEdge *errorAt, bool keepDir) const; + int AnyEdgeCrossings(Vector a, Vector b, + Vector *pi=NULL, SPointList *spl=NULL) const; + bool ContainsEdgeFrom(const SEdgeList *sel) const; + bool ContainsEdge(const SEdge *se) const; + void CullExtraneousEdges(); + void MergeCollinearSegments(Vector a, Vector b); +}; + +// A kd-tree element needs to go on a side of a node if it's when KDTREE_EPS +// of the boundary. So increasing this number never breaks anything, but may +// result in more duplicated elements. So it's conservative to be sloppy here. +#define KDTREE_EPS (20*LENGTH_EPS) + +class SEdgeLl { +public: + SEdge *se; + SEdgeLl *next; + + static SEdgeLl *Alloc(); +}; + +class SKdNodeEdges { +public: + int which; // whether c is x, y, or z + double c; + SKdNodeEdges *gt; + SKdNodeEdges *lt; + + SEdgeLl *edges; + + static SKdNodeEdges *From(SEdgeList *sel); + static SKdNodeEdges *From(SEdgeLl *sell); + static SKdNodeEdges *Alloc(); + int AnyEdgeCrossings(Vector a, Vector b, int cnt, + Vector *pi=NULL, SPointList *spl=NULL) const; +}; + +class SPoint { +public: + int tag; + + EarType ear; + + Vector p; + Vector auxv; +}; + +class SPointList { +public: + List l; + + void Clear(); + bool ContainsPoint(Vector pt) const; + int IndexForPoint(Vector pt) const; + void IncrementTagFor(Vector pt); + void Add(Vector pt); +}; + +class SContour { +public: + int tag; + int timesEnclosed; + Vector xminPt; + List l; + + void AddPoint(Vector p); + void MakeEdgesInto(SEdgeList *el) const; + void Reverse(); + Vector ComputeNormal() const; + double SignedAreaProjdToNormal(Vector n) const; + bool IsClockwiseProjdToNormal(Vector n) const; + bool ContainsPointProjdToNormal(Vector n, Vector p) const; + void OffsetInto(SContour *dest, double r) const; + void CopyInto(SContour *dest) const; + void FindPointWithMinX(); + Vector AnyEdgeMidpoint() const; + + bool IsEar(int bp, double scaledEps) const; + bool BridgeToContour(SContour *sc, SEdgeList *el, List *vl); + void ClipEarInto(SMesh *m, int bp, double scaledEps); + void UvTriangulateInto(SMesh *m, SSurface *srf); +}; + +typedef struct { + uint32_t face; + RgbaColor color; +} STriMeta; + +class SPolygon { +public: + List l; + Vector normal; + + Vector ComputeNormal() const; + void AddEmptyContour(); + int WindingNumberForPoint(Vector p) const; + double SignedArea() const; + bool ContainsPoint(Vector p) const; + void MakeEdgesInto(SEdgeList *el) const; + void FixContourDirections(); + void Clear(); + bool SelfIntersecting(Vector *intersectsAt) const; + bool IsEmpty() const; + Vector AnyPoint() const; + void OffsetInto(SPolygon *dest, double r) const; + void UvTriangulateInto(SMesh *m, SSurface *srf); + void UvGridTriangulateInto(SMesh *m, SSurface *srf); + void TriangulateInto(SMesh *m) const; + void InverseTransformInto(SPolygon *sp, Vector u, Vector v, Vector n) const; +}; + +class STriangle { +public: + int tag; + STriMeta meta; + + union { + struct { Vector a, b, c; }; + Vector vertices[3]; + }; + + union { + struct { Vector an, bn, cn; }; + Vector normals[3]; + }; + + static STriangle From(STriMeta meta, Vector a, Vector b, Vector c); + Vector Normal() const; + void FlipNormal(); + double MinAltitude() const; + int WindingNumberForPoint(Vector p) const; + bool ContainsPoint(Vector p) const; + bool ContainsPointProjd(Vector n, Vector p) const; + STriangle Transform(Vector o, Vector u, Vector v) const; + bool Raytrace(const Vector &rayPoint, const Vector &rayDir, + double *t, Vector *inters) const; + double SignedVolume() const; +}; + +class SBsp2 { +public: + Vector np; // normal to the plane + + Vector no; // outer normal to the edge + double d; + SEdge edge; + + SBsp2 *pos; + SBsp2 *neg; + + SBsp2 *more; + + void InsertTriangleHow(BspClass how, STriangle *tr, SMesh *m, SBsp3 *bsp3); + void InsertTriangle(STriangle *tr, SMesh *m, SBsp3 *bsp3); + Vector IntersectionWith(Vector a, Vector b) const; + void InsertEdge(SEdge *nedge, Vector nnp, Vector out); + static SBsp2 *InsertOrCreateEdge(SBsp2 *where, SEdge *nedge, + Vector nnp, Vector out); + static SBsp2 *Alloc(); +}; + +class SBsp3 { +public: + Vector n; + double d; + + STriangle tri; + SBsp3 *pos; + SBsp3 *neg; + + SBsp3 *more; + + SBsp2 *edges; + + static SBsp3 *Alloc(); + static SBsp3 *FromMesh(const SMesh *m); + + Vector IntersectionWith(Vector a, Vector b) const; + + void InsertHow(BspClass how, STriangle *str, SMesh *instead); + void Insert(STriangle *str, SMesh *instead); + static SBsp3 *InsertOrCreate(SBsp3 *where, STriangle *str, SMesh *instead); + + void InsertConvexHow(BspClass how, STriMeta meta, Vector *vertex, size_t n, + SMesh *instead); + SBsp3 *InsertConvex(STriMeta meta, Vector *vertex, size_t n, SMesh *instead); + + void InsertInPlane(bool pos2, STriangle *tr, SMesh *m); + + void GenerateInPaintOrder(SMesh *m) const; +}; + +class SMesh { +public: + List l; + + bool flipNormal; + bool keepCoplanar; + bool atLeastOneDiscarded; + bool isTransparent; + + void Clear(); + void AddTriangle(const STriangle *st); + void AddTriangle(STriMeta meta, Vector a, Vector b, Vector c); + void AddTriangle(STriMeta meta, Vector n, + Vector a, Vector b, Vector c); + void DoBounding(Vector v, Vector *vmax, Vector *vmin) const; + void GetBounding(Vector *vmax, Vector *vmin) const; + + void Simplify(int start); + + void AddAgainstBsp(SMesh *srcm, SBsp3 *bsp3); + void MakeFromUnionOf(SMesh *a, SMesh *b); + void MakeFromDifferenceOf(SMesh *a, SMesh *b); + + void MakeFromCopyOf(SMesh *a); + void MakeFromTransformationOf(SMesh *a, Vector trans, + Quaternion q, double scale); + void MakeFromAssemblyOf(SMesh *a, SMesh *b); + + void MakeEdgesInPlaneInto(SEdgeList *sel, Vector n, double d); + void MakeOutlinesInto(SOutlineList *sol, EdgeKind type); + + void PrecomputeTransparency(); + void RemoveDegenerateTriangles(); + + bool IsEmpty() const; + void RemapFaces(Group *g, int remap); + + uint32_t FirstIntersectionWith(Point2d mp) const; + + Vector GetCenterOfMass() const; +}; + +// A linked list of triangles +class STriangleLl { +public: + STriangle *tri; + + STriangleLl *next; + + static STriangleLl *Alloc(); +}; + +class SOutline { +public: + int tag; + Vector a, b, nl, nr; + + bool IsVisible(Vector projDir) const; +}; + +class SOutlineList { +public: + List l; + + void Clear(); + void AddEdge(Vector a, Vector b, Vector nl, Vector nr, int tag = 0); + void ListTaggedInto(SEdgeList *el, int auxA = 0, int auxB = 0); + + void MakeFromCopyOf(SOutlineList *ol); +}; + +class SKdNode { +public: + struct EdgeOnInfo { + int count; + bool frontFacing; + bool intersectsMesh; + STriangle *tr; + int ai; + int bi; + }; + + int which; // whether c is x, y, or z + double c; + + SKdNode *gt; + SKdNode *lt; + + STriangleLl *tris; + + static SKdNode *Alloc(); + static SKdNode *From(SMesh *m); + static SKdNode *From(STriangleLl *tll); + + void AddTriangle(STriangle *tr); + void MakeMeshInto(SMesh *m) const; + void ListTrianglesInto(std::vector *tl) const; + void ClearTags() const; + + void FindEdgeOn(Vector a, Vector b, int cnt, bool coplanarIsInter, EdgeOnInfo *info) const; + void MakeCertainEdgesInto(SEdgeList *sel, EdgeKind how, bool coplanarIsInter, + bool *inter, bool *leaky, int auxA = 0) const; + void MakeOutlinesInto(SOutlineList *sel, EdgeKind tagKind) const; + + void OcclusionTestLine(SEdge orig, SEdgeList *sel, int cnt) const; + void SplitLinesAgainstTriangle(SEdgeList *sel, STriangle *tr) const; + + void SnapToMesh(SMesh *m); + void SnapToVertex(Vector v, SMesh *extras); +}; + +class PolylineBuilder { +public: + struct Edge; + + struct Vertex { + Vector pos; + std::vector edges; + + bool GetNext(uint32_t kind, Vertex **next, Edge **nextEdge); + bool GetNext(uint32_t kind, Vector plane, double d, Vertex **next, Edge **nextEdge); + size_t CountEdgesWithTagAndKind(int tag, uint32_t kind) const; + }; + + struct VertexPairHash { + size_t operator()(const std::pair &v) const; + }; + + struct Edge { + Vertex *a; + Vertex *b; + uint32_t kind; + int tag; + + union { + uintptr_t data; + SOutline *outline; + SEdge *edge; + }; + + Vertex *GetOtherVertex(Vertex *v) const; + bool GetStartAndNext(Vertex **start, Vertex **next, bool loop) const; + }; + + std::unordered_map vertices; + std::unordered_map, Edge *, VertexPairHash> edgeMap; + std::vector edges; + + ~PolylineBuilder(); + void Clear(); + + Vertex *AddVertex(const Vector &pos); + Edge *AddEdge(const Vector &p0, const Vector &p1, uint32_t kind, uintptr_t data = 0); + void Generate( + std::function startFunc, + std::function nextFunc, + std::function aloneFunc, + std::function endFunc = [](){}); + + void MakeFromEdges(const SEdgeList &sel); + void MakeFromOutlines(const SOutlineList &sol); + void GenerateEdges(SEdgeList *sel); + void GenerateOutlines(SOutlineList *sol); +}; + +#endif + diff --git a/ThirdParty/libslvs/render/render.h b/ThirdParty/libslvs/render/render.h new file mode 100644 index 0000000000..07eeca4db6 --- /dev/null +++ b/ThirdParty/libslvs/render/render.h @@ -0,0 +1,366 @@ +//----------------------------------------------------------------------------- +// Backend-agnostic rendering interface, and various backends we use. +// +// Copyright 2016 whitequark +//----------------------------------------------------------------------------- + +#ifndef SOLVESPACE_RENDER_H +#define SOLVESPACE_RENDER_H + +//----------------------------------------------------------------------------- +// Interfaces and utilities common for all renderers. +//----------------------------------------------------------------------------- + +enum class StipplePattern : uint32_t; + +// A mapping from 3d sketch coordinates to 2d screen coordinates, using +// an axonometric projection. +class Camera { +public: + size_t width, height; + Vector offset; + Vector projRight; + Vector projUp; + double scale; + double tangent; + bool hasPixels; + + bool IsPerspective() const { return tangent != 0.0; } + + Point2d ProjectPoint(Vector p) const; + Vector ProjectPoint3(Vector p) const; + Vector ProjectPoint4(Vector p, double *w) const; + Vector UnProjectPoint(Point2d p) const; + Vector UnProjectPoint3(Vector p) const; + Vector VectorFromProjs(Vector rightUpForward) const; + Vector AlignToPixelGrid(Vector v) const; + + SBezier ProjectBezier(SBezier b) const; + + void LoadIdentity(); + void NormalizeProjectionVectors(); +}; + +// A description of scene lighting. +class Lighting { +public: + RgbaColor backgroundColor; + double ambientIntensity; + double lightIntensity[2]; + Vector lightDirection[2]; +}; + +class BatchCanvas; + +// An interface for populating a drawing area with geometry. +class Canvas { +public: + // Stroke and fill styles are addressed with handles to be able to quickly + // group geometry into indexed draw calls. + class hStroke { + public: + uint32_t v; + }; + + class hFill { + public: + uint32_t v; + }; + + // The layer of a geometry describes how it occludes other geometry. + // Within a layer, geometry with higher z-index occludes geometry with lower z-index, + // or geometry drawn earlier if z-indexes match. + enum class Layer { + NORMAL, // Occluded by geometry with lower Z coordinate + OCCLUDED, // Only drawn over geometry with lower Z coordinate + DEPTH_ONLY, // Like NORMAL, but only affects future occlusion, not color + BACK, // Always drawn below all other geometry + FRONT, // Always drawn above all other geometry + LAST = FRONT + }; + + // The outlines are the collection of all edges that may be drawn. + // Outlines can be classified as emphasized or not; emphasized outlines indicate an abrupt + // change in the surface curvature. These are indicated by the SOutline tag. + // Outlines can also be classified as contour or not; contour outlines indicate the boundary + // of the filled mesh. Whether an outline is a part of contour or not depends on point of view. + enum class DrawOutlinesAs { + EMPHASIZED_AND_CONTOUR = 0, // Both emphasized and contour outlines + EMPHASIZED_WITHOUT_CONTOUR = 1, // Emphasized outlines except those also belonging to contour + CONTOUR_ONLY = 2 // Contour outlines only + }; + + // Stroke widths, etc, can be scale-invariant (in pixels) or scale-dependent (in millimeters). + enum class Unit { + MM, + PX + }; + + class Stroke { + public: + hStroke h; + + Layer layer; + int zIndex; + RgbaColor color; + double width; + Unit unit; + StipplePattern stipplePattern; + double stippleScale; + + void Clear() { *this = {}; } + bool Equals(const Stroke &other) const; + + double WidthMm(const Camera &camera) const; + double WidthPx(const Camera &camera) const; + double StippleScaleMm(const Camera &camera) const; + double StippleScalePx(const Camera &camera) const; + }; + + enum class FillPattern { + SOLID, CHECKERED_A, CHECKERED_B + }; + + class Fill { + public: + hFill h; + + Layer layer; + int zIndex; + RgbaColor color; + FillPattern pattern; + std::shared_ptr texture; + + void Clear() { *this = {}; } + bool Equals(const Fill &other) const; + }; + + IdList strokes; + IdList fills; + BitmapFont bitmapFont; + + Canvas() : strokes(), fills(), bitmapFont() {} + virtual void Clear(); + + hStroke GetStroke(const Stroke &stroke); + hFill GetFill(const Fill &fill); + BitmapFont *GetBitmapFont(); + + virtual const Camera &GetCamera() const = 0; + + virtual void DrawLine(const Vector &a, const Vector &b, hStroke hcs) = 0; + virtual void DrawEdges(const SEdgeList &el, hStroke hcs) = 0; + virtual bool DrawBeziers(const SBezierList &bl, hStroke hcs) = 0; + virtual void DrawOutlines(const SOutlineList &ol, hStroke hcs, DrawOutlinesAs drawAs) = 0; + virtual void DrawVectorText(const std::string &text, double height, + const Vector &o, const Vector &u, const Vector &v, + hStroke hcs) = 0; + + virtual void DrawQuad(const Vector &a, const Vector &b, const Vector &c, const Vector &d, + hFill hcf) = 0; + virtual void DrawPoint(const Vector &o, hStroke hcs) = 0; + virtual void DrawPolygon(const SPolygon &p, hFill hcf) = 0; + virtual void DrawMesh(const SMesh &m, hFill hcfFront, hFill hcfBack = {}) = 0; + virtual void DrawFaces(const SMesh &m, const std::vector &faces, hFill hcf) = 0; + + virtual void DrawPixmap(std::shared_ptr pm, + const Vector &o, const Vector &u, const Vector &v, + const Point2d &ta, const Point2d &tb, hFill hcf) = 0; + virtual void InvalidatePixmap(std::shared_ptr pm) = 0; + + virtual std::shared_ptr CreateBatch(); +}; + +// An interface for view-dependent visualization. +class ViewportCanvas : public Canvas { +public: + virtual void SetCamera(const Camera &camera) = 0; + virtual void SetLighting(const Lighting &lighting) = 0; + + virtual void NewFrame() = 0; + virtual void FlushFrame() = 0; + virtual std::shared_ptr ReadFrame() = 0; + + virtual void GetIdent(const char **vendor, const char **renderer, const char **version) = 0; +}; + +// An interface for view-independent visualization. +class BatchCanvas : public Canvas { +public: + const Camera &GetCamera() const override; + + virtual void Finalize() = 0; + virtual void Draw() = 0; +}; + +// A wrapper around Canvas that simplifies drawing UI in screen coordinates. +class UiCanvas { +public: + std::shared_ptr canvas; + bool flip; + + void DrawLine(int x1, int y1, int x2, int y2, RgbaColor color, int width = 1, + int zIndex = 0); + void DrawRect(int l, int r, int t, int b, RgbaColor fillColor, RgbaColor outlineColor, + int zIndex = 0); + void DrawPixmap(std::shared_ptr pm, int x, int y, + int zIndex = 0); + void DrawBitmapChar(char32_t codepoint, int x, int y, RgbaColor color, + int zIndex = 0); + void DrawBitmapText(const std::string &str, int x, int y, RgbaColor color, + int zIndex = 0); + + int Flip(int y) const { return flip ? (int)canvas->GetCamera().height - y : y; } +}; + +// A canvas that performs picking against drawn geometry. +class ObjectPicker : public Canvas { +public: + Camera camera; + // Configuration. + Point2d point; + double selRadius; + // Picking state. + double minDistance; + int maxZIndex; + uint32_t position; + + ObjectPicker() : camera(), point(), selRadius(), + minDistance(), maxZIndex(), position() {} + + const Camera &GetCamera() const override { return camera; } + + void DrawLine(const Vector &a, const Vector &b, hStroke hcs) override; + void DrawEdges(const SEdgeList &el, hStroke hcs) override; + bool DrawBeziers(const SBezierList &bl, hStroke hcs) override { return false; } + void DrawOutlines(const SOutlineList &ol, hStroke hcs, DrawOutlinesAs drawAs) override; + void DrawVectorText(const std::string &text, double height, + const Vector &o, const Vector &u, const Vector &v, + hStroke hcs) override; + + void DrawQuad(const Vector &a, const Vector &b, const Vector &c, const Vector &d, + hFill hcf) override; + void DrawPoint(const Vector &o, hStroke hcs) override; + void DrawPolygon(const SPolygon &p, hFill hcf) override; + void DrawMesh(const SMesh &m, hFill hcfFront, hFill hcfBack) override; + void DrawFaces(const SMesh &m, const std::vector &faces, hFill hcf) override; + + void DrawPixmap(std::shared_ptr pm, + const Vector &o, const Vector &u, const Vector &v, + const Point2d &ta, const Point2d &tb, hFill hcf) override; + void InvalidatePixmap(std::shared_ptr pm) override {} + + void DoCompare(double distance, int zIndex, int comparePosition = 0); + void DoQuad(const Vector &a, const Vector &b, const Vector &c, const Vector &d, + int zIndex, int comparePosition = 0); + + bool Pick(std::function drawFn); +}; + +// A canvas that renders onto a 2d surface, performing z-index sorting, occlusion testing, etc, +// on the CPU. +class SurfaceRenderer : public Canvas { +public: + Camera camera; + Lighting lighting; + // Chord tolerance, for converting beziers to pwl. + double chordTolerance; + // Render lists. + handle_map edges; + handle_map beziers; + SMesh mesh; + // State. + BBox bbox; + + SurfaceRenderer() : camera(), lighting(), chordTolerance(), mesh(), bbox() {} + void Clear() override; + + // Canvas interface. + const Camera &GetCamera() const override { return camera; } + + void DrawLine(const Vector &a, const Vector &b, hStroke hcs) override; + void DrawEdges(const SEdgeList &el, hStroke hcs) override; + bool DrawBeziers(const SBezierList &bl, hStroke hcs) override; + void DrawOutlines(const SOutlineList &ol, hStroke hcs, DrawOutlinesAs drawAs) override; + void DrawVectorText(const std::string &text, double height, + const Vector &o, const Vector &u, const Vector &v, + hStroke hcs) override; + + void DrawQuad(const Vector &a, const Vector &b, const Vector &c, const Vector &d, + hFill hcf) override; + void DrawPoint(const Vector &o, hStroke hcs) override; + void DrawPolygon(const SPolygon &p, hFill hcf) override; + void DrawMesh(const SMesh &m, hFill hcfFront, hFill hcfBack) override; + void DrawFaces(const SMesh &m, const std::vector &faces, hFill hcf) override; + + void DrawPixmap(std::shared_ptr pm, + const Vector &o, const Vector &u, const Vector &v, + const Point2d &ta, const Point2d &tb, hFill hcf) override; + void InvalidatePixmap(std::shared_ptr pm) override; + + // Geometry manipulation. + void CalculateBBox(); + void ConvertBeziersToEdges(); + void CullOccludedStrokes(); + + // Renderer operations. + void OutputInPaintOrder(); + + virtual bool CanOutputCurves() const = 0; + virtual bool CanOutputTriangles() const = 0; + + virtual void OutputStart() = 0; + virtual void OutputBezier(const SBezier &b, hStroke hcs) = 0; + virtual void OutputTriangle(const STriangle &tr) = 0; + virtual void OutputEnd() = 0; + + void OutputBezierAsNonrationalCubic(const SBezier &b, hStroke hcs); +}; + +//----------------------------------------------------------------------------- +// 2d renderers. +//----------------------------------------------------------------------------- + +class CairoRenderer : public SurfaceRenderer { +public: + cairo_t *context; + // Renderer configuration. + bool antialias; + // Renderer state. + struct { + hStroke hcs; + } current; + + CairoRenderer() : context(), current() {} + + void SelectStroke(hStroke hcs); + void MoveTo(Vector p); + void FinishPath(); + + bool CanOutputCurves() const override { return true; } + bool CanOutputTriangles() const override { return true; } + + void OutputStart() override; + void OutputBezier(const SBezier &b, hStroke hcs) override; + void OutputTriangle(const STriangle &tr) override; + void OutputEnd() override; +}; + +//----------------------------------------------------------------------------- +// 3d renderers. +//----------------------------------------------------------------------------- + +// An offscreen renderer based on OpenGL framebuffers. +class GlOffscreen { +public: + unsigned int framebuffer; + unsigned int colorRenderbuffer, depthRenderbuffer; + std::vector data; + + bool Render(int width, int height, std::function renderFn); + void Clear(); +}; + +std::shared_ptr CreateRenderer(); + +#endif diff --git a/ThirdParty/libslvs/resource.h b/ThirdParty/libslvs/resource.h new file mode 100644 index 0000000000..20bf6bd4ea --- /dev/null +++ b/ThirdParty/libslvs/resource.h @@ -0,0 +1,109 @@ +//----------------------------------------------------------------------------- +// Discovery and loading of our resources (icons, fonts, templates, etc). +// +// Copyright 2016 whitequark +//----------------------------------------------------------------------------- + +#ifndef __RESOURCE_H +#define __RESOURCE_H + +class Camera; +class Point2d; +class Pixmap; +class Vector; + +std::string LoadString(const std::string &name); +std::string LoadStringFromGzip(const std::string &name); +std::shared_ptr LoadPng(const std::string &name); + +class Pixmap { +public: + enum class Format { BGRA, RGBA, BGR, RGB, A }; + + Format format; + size_t width; + size_t height; + size_t stride; + std::vector data; + + static std::shared_ptr Create(Format format, size_t width, size_t height); + static std::shared_ptr FromPng(const uint8_t *data, size_t size, bool flip = false); + + static std::shared_ptr ReadPng(FILE *f, bool flip = false); + static std::shared_ptr ReadPng(const Platform::Path &filename, bool flip = false); + bool WritePng(FILE *f, bool flip = false); + bool WritePng(const Platform::Path &filename, bool flip = false); + + size_t GetBytesPerPixel() const; + RgbaColor GetPixel(size_t x, size_t y) const; + bool Equals(const Pixmap &other) const; + + void ConvertTo(Format newFormat); + void SetPixel(size_t x, size_t y, RgbaColor color); +}; + +class BitmapFont { +public: + struct Glyph { + uint8_t advanceCells; + uint16_t position; + }; + + std::string unifontData; + std::map glyphs; + std::shared_ptr texture; + bool textureUpdated; + uint16_t nextPosition; + + static BitmapFont From(std::string &&unifontData); + static BitmapFont Create(); + + bool IsEmpty() const { return unifontData.empty(); } + const Glyph &GetGlyph(char32_t codepoint); + void LocateGlyph(char32_t codepoint, double *s0, double *t0, double *s1, double *t1, + size_t *advanceWidth, size_t *boundingHeight); + + void AddGlyph(char32_t codepoint, std::shared_ptr pixmap); + + size_t GetWidth(char32_t codepoint); + size_t GetWidth(const std::string &str); +}; + +class VectorFont { +public: + struct Contour { + std::vector points; + }; + + struct Glyph { + std::vector contours; + double leftSideBearing; + double boundingWidth; + double advanceWidth; + }; + + std::string lffData; + std::map glyphs; + double rightSideBearing; + double capHeight; + double ascender; + double descender; + + static VectorFont From(std::string &&lffData); + static VectorFont *Builtin(); + + bool IsEmpty() const { return lffData.empty(); } + const Glyph &GetGlyph(char32_t codepoint); + + double GetCapHeight(double forCapHeight) const; + double GetHeight(double forCapHeight) const; + double GetWidth(double forCapHeight, const std::string &str); + Vector GetExtents(double forCapHeight, const std::string &str); + + void Trace(double forCapHeight, Vector o, Vector u, Vector v, const std::string &str, + std::function traceEdge); + void Trace(double forCapHeight, Vector o, Vector u, Vector v, const std::string &str, + std::function traceEdge, const Camera &camera); +}; + +#endif diff --git a/ThirdParty/libslvs/sketch.h b/ThirdParty/libslvs/sketch.h new file mode 100644 index 0000000000..65cfd2ec06 --- /dev/null +++ b/ThirdParty/libslvs/sketch.h @@ -0,0 +1,926 @@ +//----------------------------------------------------------------------------- +// The parametric structure of our sketch, in multiple groups, that generate +// geometric entities and surfaces. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- + +#ifndef __SKETCH_H +#define __SKETCH_H + +class hGroup; +class hRequest; +class hEntity; +class hParam; +class hStyle; +class hConstraint; +class hEquation; + +class Entity; +class Param; +class Equation; +class Style; + +enum class PolyError : uint32_t { + GOOD = 0, + NOT_CLOSED = 1, + NOT_COPLANAR = 2, + SELF_INTERSECTING = 3, + ZERO_LEN_EDGE = 4 +}; + +enum class StipplePattern : uint32_t { + CONTINUOUS = 0, + SHORT_DASH = 1, + DASH = 2, + LONG_DASH = 3, + DASH_DOT = 4, + DASH_DOT_DOT = 5, + DOT = 6, + FREEHAND = 7, + ZIGZAG = 8, + + LAST = ZIGZAG +}; + +const std::vector &StipplePatternDashes(StipplePattern pattern); +double StipplePatternLength(StipplePattern pattern); + +enum class Command : uint32_t; + +// All of the hWhatever handles are a 32-bit ID, that is used to represent +// some data structure in the sketch. +class hGroup { +public: + // bits 15: 0 -- group index + uint32_t v; + + inline hEntity entity(int i) const; + inline hParam param(int i) const; + inline hEquation equation(int i) const; +}; +class hRequest { +public: + // bits 15: 0 -- request index + uint32_t v; + + inline hEntity entity(int i) const; + inline hParam param(int i) const; + + inline bool IsFromReferences() const; +}; +class hEntity { +public: + // bits 15: 0 -- entity index + // 31:16 -- request index + uint32_t v; + + inline bool isFromRequest() const; + inline hRequest request() const; + inline hGroup group() const; + inline hEquation equation(int i) const; +}; +class hParam { +public: + // bits 15: 0 -- param index + // 31:16 -- request index + uint32_t v; + + inline hRequest request() const; +}; + +class hStyle { +public: + uint32_t v; +}; + +class EntityId { +public: + uint32_t v; // entity ID, starting from 0 +}; +class EntityMap { +public: + int tag; + + EntityId h; + hEntity input; + int copyNumber; + // (input, copyNumber) gets mapped to ((Request)xxx).entity(h.v) + + void Clear() {} +}; + +// A set of requests. Every request must have an associated group. +class Group { +public: + static const hGroup HGROUP_REFERENCES; + + int tag; + hGroup h; + + enum class CopyAs { + NUMERIC, + N_TRANS, + N_ROT_AA, + N_ROT_TRANS, + }; + + enum class Type : uint32_t { + DRAWING_3D = 5000, + DRAWING_WORKPLANE = 5001, + EXTRUDE = 5100, + LATHE = 5101, + ROTATE = 5200, + TRANSLATE = 5201, + LINKED = 5300 + }; + Group::Type type; + + int order; + + hGroup opA; + hGroup opB; + bool visible; + bool suppress; + bool relaxConstraints; + bool allowRedundant; + bool allDimsReference; + double scale; + + bool clean; + bool dofCheckOk; + hEntity activeWorkplane; + double valA; + double valB; + double valC; + RgbaColor color; + + struct { + SolveResult how; + int dof; + List remove; + } solved; + + enum class Subtype : uint32_t { + // For drawings in 2d + WORKPLANE_BY_POINT_ORTHO = 6000, + WORKPLANE_BY_LINE_SEGMENTS = 6001, + // For extrudes, translates, and rotates + ONE_SIDED = 7000, + TWO_SIDED = 7001 + }; + Group::Subtype subtype; + + bool skipFirst; // for step and repeat ops + + struct { + Quaternion q; + hEntity origin; + hEntity entityB; + hEntity entityC; + bool swapUV; + bool negateU; + bool negateV; + } predef; + + SPolygon polyLoops; + SBezierLoopSetSet bezierLoops; + SBezierList bezierOpens; + + struct { + PolyError how; + SEdge notClosedAt; + Vector errorPointAt; + } polyError; + + bool booleanFailed; + + SShell thisShell; + SShell runningShell; + + SMesh thisMesh; + SMesh runningMesh; + + bool displayDirty; + SMesh displayMesh; + SOutlineList displayOutlines; + + enum class CombineAs : uint32_t { + UNION = 0, + DIFFERENCE = 1, + ASSEMBLE = 2 + }; + CombineAs meshCombine; + + bool forceToMesh; + + IdList remap; + enum { REMAP_PRIME = 19477 }; + int remapCache[REMAP_PRIME]; + + Platform::Path linkFile; + SMesh impMesh; + SShell impShell; + EntityList impEntity; + + std::string name; + + + void Activate(); + std::string DescriptionString(); + void Clear(); + + static void AddParam(ParamList *param, hParam hp, double v); + void Generate(EntityList *entity, ParamList *param); + bool IsSolvedOkay(); + void TransformImportedBy(Vector t, Quaternion q); + bool IsForcedToMeshBySource() const; + bool IsForcedToMesh() const; + // When a request generates entities from entities, and the source + // entities may have come from multiple requests, it's necessary to + // remap the entity ID so that it's still unique. We do this with a + // mapping list. + enum { + REMAP_LAST = 1000, + REMAP_TOP = 1001, + REMAP_BOTTOM = 1002, + REMAP_PT_TO_LINE = 1003, + REMAP_LINE_TO_FACE = 1004, + REMAP_LATHE_START = 1006, + REMAP_LATHE_END = 1007, + REMAP_PT_TO_ARC = 1008, + REMAP_PT_TO_NORMAL = 1009, + }; + hEntity Remap(hEntity in, int copyNumber); + void MakeExtrusionLines(EntityList *el, hEntity in); + void MakeLatheCircles(IdList *el, IdList *param, hEntity in, Vector pt, Vector axis, int ai); + void MakeExtrusionTopBottomFaces(EntityList *el, hEntity pt); + void CopyEntity(EntityList *el, + Entity *ep, int timesApplied, int remap, + hParam dx, hParam dy, hParam dz, + hParam qw, hParam qvx, hParam qvy, hParam qvz, + CopyAs as); + + void AddEq(IdList *l, Expr *expr, int index); + void GenerateEquations(IdList *l); + bool IsVisible(); + int GetNumConstraints(); + Vector ExtrusionGetVector(); + void ExtrusionForceVectorTo(const Vector &v); + + // Assembling the curves into loops, and into a piecewise linear polygon + // at the same time. + void AssembleLoops(bool *allClosed, bool *allCoplanar, bool *allNonZeroLen); + void GenerateLoops(); + // And the mesh stuff + Group *PreviousGroup() const; + Group *RunningMeshGroup() const; + bool IsMeshGroup(); + + void GenerateShellAndMesh(); + template void GenerateForStepAndRepeat(T *steps, T *outs, Group::CombineAs forWhat); + template void GenerateForBoolean(T *a, T *b, T *o, Group::CombineAs how); + void GenerateDisplayItems(); + + enum class DrawMeshAs { DEFAULT, HOVERED, SELECTED }; + void DrawMesh(DrawMeshAs how, Canvas *canvas); + void Draw(Canvas *canvas); + void DrawPolyError(Canvas *canvas); + void DrawFilledPaths(Canvas *canvas); + void DrawContourAreaLabels(Canvas *canvas); + + SPolygon GetPolygon(); + + static void MenuGroup(Command id); +}; + +// A user request for some primitive or derived operation; for example a +// line, or a step and repeat. +class Request { +public: + // Some predefined requests, that are present in every sketch. + static const hRequest HREQUEST_REFERENCE_XY; + static const hRequest HREQUEST_REFERENCE_YZ; + static const hRequest HREQUEST_REFERENCE_ZX; + + int tag; + hRequest h; + + // Types of requests + enum class Type : uint32_t { + WORKPLANE = 100, + DATUM_POINT = 101, + LINE_SEGMENT = 200, + CUBIC = 300, + CUBIC_PERIODIC = 301, + CIRCLE = 400, + ARC_OF_CIRCLE = 500, + TTF_TEXT = 600, + IMAGE = 700 + }; + + Request::Type type; + int extraPoints; + + hEntity workplane; // or Entity::FREE_IN_3D + hGroup group; + hStyle style; + + bool construction; + + std::string str; + std::string font; + Platform::Path file; + double aspectRatio; + + static hParam AddParam(ParamList *param, hParam hp); + void Generate(EntityList *entity, ParamList *param); + + std::string DescriptionString() const; + int IndexOfPoint(hEntity he) const; + + void Clear() {} +}; + +#define MAX_POINTS_IN_ENTITY (12) +class EntityBase { +public: + int tag; + hEntity h; + + static const hEntity FREE_IN_3D; + static const hEntity NO_ENTITY; + + enum class Type : uint32_t { + POINT_IN_3D = 2000, + POINT_IN_2D = 2001, + POINT_N_TRANS = 2010, + POINT_N_ROT_TRANS = 2011, + POINT_N_COPY = 2012, + POINT_N_ROT_AA = 2013, + + NORMAL_IN_3D = 3000, + NORMAL_IN_2D = 3001, + NORMAL_N_COPY = 3010, + NORMAL_N_ROT = 3011, + NORMAL_N_ROT_AA = 3012, + + DISTANCE = 4000, + DISTANCE_N_COPY = 4001, + + FACE_NORMAL_PT = 5000, + FACE_XPROD = 5001, + FACE_N_ROT_TRANS = 5002, + FACE_N_TRANS = 5003, + FACE_N_ROT_AA = 5004, + + WORKPLANE = 10000, + LINE_SEGMENT = 11000, + CUBIC = 12000, + CUBIC_PERIODIC = 12001, + CIRCLE = 13000, + ARC_OF_CIRCLE = 14000, + TTF_TEXT = 15000, + IMAGE = 16000 + }; + + Type type; + + hGroup group; + hEntity workplane; // or Entity::FREE_IN_3D + + // When it comes time to draw an entity, we look here to get the + // defining variables. + hEntity point[MAX_POINTS_IN_ENTITY]; + int extraPoints; + hEntity normal; + hEntity distance; + // The only types that have their own params are points, normals, + // and directions. + hParam param[7]; + + // Transformed points/normals/distances have their numerical base + Vector numPoint; + Quaternion numNormal; + double numDistance; + + std::string str; + std::string font; + Platform::Path file; + double aspectRatio; + + // For entities that are derived by a transformation, the number of + // times to apply the transformation. + int timesApplied; + + Quaternion GetAxisAngleQuaternion(int param0) const; + ExprQuaternion GetAxisAngleQuaternionExprs(int param0) const; + + bool IsCircle() const; + Expr *CircleGetRadiusExpr() const; + double CircleGetRadiusNum() const; + void ArcGetAngles(double *thetaa, double *thetab, double *dtheta) const; + + bool HasVector() const; + ExprVector VectorGetExprs() const; + ExprVector VectorGetExprsInWorkplane(hEntity wrkpl) const; + Vector VectorGetNum() const; + Vector VectorGetRefPoint() const; + Vector VectorGetStartPoint() const; + + // For distances + bool IsDistance() const; + double DistanceGetNum() const; + Expr *DistanceGetExpr() const; + void DistanceForceTo(double v); + + bool IsWorkplane() const; + // The plane is points P such that P dot (xn, yn, zn) - d = 0 + void WorkplaneGetPlaneExprs(ExprVector *n, Expr **d) const; + ExprVector WorkplaneGetOffsetExprs() const; + Vector WorkplaneGetOffset() const; + EntityBase *Normal() const; + + bool IsFace() const; + ExprVector FaceGetNormalExprs() const; + Vector FaceGetNormalNum() const; + ExprVector FaceGetPointExprs() const; + Vector FaceGetPointNum() const; + + bool IsPoint() const; + // Applies for any of the point types + Vector PointGetNum() const; + ExprVector PointGetExprs() const; + void PointGetExprsInWorkplane(hEntity wrkpl, Expr **u, Expr **v) const; + ExprVector PointGetExprsInWorkplane(hEntity wrkpl) const; + void PointForceTo(Vector v); + void PointForceParamTo(Vector v); + // These apply only the POINT_N_ROT_TRANS, which has an assoc rotation + Quaternion PointGetQuaternion() const; + void PointForceQuaternionTo(Quaternion q); + + bool IsNormal() const; + // Applies for any of the normal types + Quaternion NormalGetNum() const; + ExprQuaternion NormalGetExprs() const; + void NormalForceTo(Quaternion q); + + Vector NormalU() const; + Vector NormalV() const; + Vector NormalN() const; + ExprVector NormalExprsU() const; + ExprVector NormalExprsV() const; + ExprVector NormalExprsN() const; + + Vector CubicGetStartNum() const; + Vector CubicGetFinishNum() const; + ExprVector CubicGetStartTangentExprs() const; + ExprVector CubicGetFinishTangentExprs() const; + Vector CubicGetStartTangentNum() const; + Vector CubicGetFinishTangentNum() const; + + bool HasEndpoints() const; + Vector EndpointStart() const; + Vector EndpointFinish() const; + + void RectGetPointsExprs(ExprVector *eap, ExprVector *ebp) const; + + void AddEq(IdList *l, Expr *expr, int index) const; + void GenerateEquations(IdList *l) const; + + void Clear() {} +}; + +class Entity : public EntityBase { +public: + // Necessary for Entity e = {} to zero-initialize, since + // classes with base classes are not aggregates and + // the default constructor does not initialize members. + // + // Note EntityBase({}); without explicitly value-initializing + // the base class, MSVC2013 will default-initialize it, leaving + // POD members with indeterminate value. + Entity() : EntityBase({}), forceHidden(), actPoint(), actNormal(), + actDistance(), actVisible(), style(), construction(), + beziers(), edges(), edgesChordTol(), screenBBox(), screenBBoxValid() {}; + + // A linked entity that was hidden in the source file ends up hidden + // here too. + bool forceHidden; + + // All points/normals/distances have their numerical value; this is + // a convenience, to simplify the link/assembly code, so that the + // part is entirely described by the entities. + Vector actPoint; + Quaternion actNormal; + double actDistance; + // and the shown state also gets saved here, for later import + bool actVisible; + + hStyle style; + bool construction; + + SBezierList beziers; + SEdgeList edges; + double edgesChordTol; + BBox screenBBox; + bool screenBBoxValid; + + bool IsStylable() const; + bool IsVisible() const; + + enum class DrawAs { DEFAULT, OVERLAY, HIDDEN, HOVERED, SELECTED }; + void Draw(DrawAs how, Canvas *canvas); + void GetReferencePoints(std::vector *refs); + int GetPositionOfPoint(const Camera &camera, Point2d p); + + void ComputeInterpolatingSpline(SBezierList *sbl, bool periodic) const; + void GenerateBezierCurves(SBezierList *sbl) const; + void GenerateEdges(SEdgeList *el); + + SBezierList *GetOrGenerateBezierCurves(); + SEdgeList *GetOrGenerateEdges(); + BBox GetOrGenerateScreenBBox(bool *hasBBox); + + void CalculateNumerical(bool forExport); + + std::string DescriptionString() const; + + void Clear() { + beziers.l.Clear(); + edges.l.Clear(); + } +}; + +class EntReqTable { +public: + static bool GetRequestInfo(Request::Type req, int extraPoints, + EntityBase::Type *ent, int *pts, bool *hasNormal, bool *hasDistance); + static bool GetEntityInfo(EntityBase::Type ent, int extraPoints, + Request::Type *req, int *pts, bool *hasNormal, bool *hasDistance); + static Request::Type GetRequestForEntity(EntityBase::Type ent); +}; + +class Param { +public: + int tag; + hParam h; + + double val; + bool known; + bool free; + + // Used only in the solver + hParam substd; + + static const hParam NO_PARAM; + + void Clear() {} +}; + + +class hConstraint { +public: + uint32_t v; + + inline hEquation equation(int i) const; + inline hParam param(int i) const; +}; + +class ConstraintBase { +public: + int tag; + hConstraint h; + + static const hConstraint NO_CONSTRAINT; + + enum class Type : uint32_t { + POINTS_COINCIDENT = 20, + PT_PT_DISTANCE = 30, + PT_PLANE_DISTANCE = 31, + PT_LINE_DISTANCE = 32, + PT_FACE_DISTANCE = 33, + PROJ_PT_DISTANCE = 34, + PT_IN_PLANE = 41, + PT_ON_LINE = 42, + PT_ON_FACE = 43, + EQUAL_LENGTH_LINES = 50, + LENGTH_RATIO = 51, + EQ_LEN_PT_LINE_D = 52, + EQ_PT_LN_DISTANCES = 53, + EQUAL_ANGLE = 54, + EQUAL_LINE_ARC_LEN = 55, + LENGTH_DIFFERENCE = 56, + SYMMETRIC = 60, + SYMMETRIC_HORIZ = 61, + SYMMETRIC_VERT = 62, + SYMMETRIC_LINE = 63, + AT_MIDPOINT = 70, + HORIZONTAL = 80, + VERTICAL = 81, + DIAMETER = 90, + PT_ON_CIRCLE = 100, + SAME_ORIENTATION = 110, + ANGLE = 120, + PARALLEL = 121, + PERPENDICULAR = 122, + ARC_LINE_TANGENT = 123, + CUBIC_LINE_TANGENT = 124, + CURVE_CURVE_TANGENT = 125, + EQUAL_RADIUS = 130, + WHERE_DRAGGED = 200, + + COMMENT = 1000 + }; + + Type type; + + hGroup group; + hEntity workplane; + + // These are the parameters for the constraint. + double valA; + hParam valP; + hEntity ptA; + hEntity ptB; + hEntity entityA; + hEntity entityB; + hEntity entityC; + hEntity entityD; + bool other; + bool other2; + + bool reference; // a ref dimension, that generates no eqs + std::string comment; // since comments are represented as constraints + + bool HasLabel() const; + + void Generate(IdList *param); + + void GenerateEquations(IdList *entity, + bool forReference = false) const; + // Some helpers when generating symbolic constraint equations + void ModifyToSatisfy(); + void AddEq(IdList *l, Expr *expr, int index) const; + void AddEq(IdList *l, const ExprVector &v, int baseIndex = 0) const; + static Expr *DirectionCosine(hEntity wrkpl, ExprVector ae, ExprVector be); + static Expr *Distance(hEntity workplane, hEntity pa, hEntity pb); + static Expr *PointLineDistance(hEntity workplane, hEntity pt, hEntity ln); + static Expr *PointPlaneDistance(ExprVector p, hEntity plane); + static ExprVector VectorsParallel3d(ExprVector a, ExprVector b, hParam p); + static ExprVector PointInThreeSpace(hEntity workplane, Expr *u, Expr *v); + + void Clear() {} +}; + +class Constraint : public ConstraintBase { +public: + // See Entity::Entity(). + Constraint() : ConstraintBase({}), disp() {} + + // These define how the constraint is drawn on-screen. + struct { + Vector offset; + hStyle style; + } disp; + + bool IsVisible() const; + bool IsStylable() const; + hStyle GetStyle() const; + bool HasLabel() const; + std::string Label() const; + + enum class DrawAs { DEFAULT, HOVERED, SELECTED }; + void Draw(DrawAs how, Canvas *canvas); + Vector GetLabelPos(const Camera &camera); + void GetReferencePoints(const Camera &camera, std::vector *refs); + + void DoLayout(DrawAs how, Canvas *canvas, + Vector *labelPos, std::vector *refs); + void DoLine(Canvas *canvas, Canvas::hStroke hcs, Vector a, Vector b); + void DoStippledLine(Canvas *canvas, Canvas::hStroke hcs, Vector a, Vector b); + bool DoLineExtend(Canvas *canvas, Canvas::hStroke hcs, + Vector p0, Vector p1, Vector pt, double salient); + void DoArcForAngle(Canvas *canvas, Canvas::hStroke hcs, + Vector a0, Vector da, Vector b0, Vector db, + Vector offset, Vector *ref, bool trim); + void DoArrow(Canvas *canvas, Canvas::hStroke hcs, + Vector p, Vector dir, Vector n, double width, double angle, double da); + void DoLineWithArrows(Canvas *canvas, Canvas::hStroke hcs, + Vector ref, Vector a, Vector b, bool onlyOneExt); + int DoLineTrimmedAgainstBox(Canvas *canvas, Canvas::hStroke hcs, + Vector ref, Vector a, Vector b, bool extend, + Vector gr, Vector gu, double swidth, double sheight); + int DoLineTrimmedAgainstBox(Canvas *canvas, Canvas::hStroke hcs, + Vector ref, Vector a, Vector b, bool extend = true); + void DoLabel(Canvas *canvas, Canvas::hStroke hcs, + Vector ref, Vector *labelPos, Vector gr, Vector gu); + void DoProjectedPoint(Canvas *canvas, Canvas::hStroke hcs, Vector *p); + void DoProjectedPoint(Canvas *canvas, Canvas::hStroke hcs, Vector *p, Vector n, Vector o); + + void DoEqualLenTicks(Canvas *canvas, Canvas::hStroke hcs, + Vector a, Vector b, Vector gn, Vector *refp); + void DoEqualRadiusTicks(Canvas *canvas, Canvas::hStroke hcs, + hEntity he, Vector *refp); + + std::string DescriptionString() const; + + static hConstraint AddConstraint(Constraint *c, bool rememberForUndo); + static hConstraint AddConstraint(Constraint *c); + static void MenuConstrain(Command id); + static void DeleteAllConstraintsFor(Constraint::Type type, hEntity entityA, hEntity ptA); + + static hConstraint ConstrainCoincident(hEntity ptA, hEntity ptB); + static hConstraint Constrain(Constraint::Type type, hEntity ptA, hEntity ptB, hEntity entityA); + static hConstraint Constrain(Constraint::Type type, hEntity ptA, hEntity ptB, + hEntity entityA, hEntity entityB, + bool other, bool other2); +}; + +class hEquation { +public: + uint32_t v; + + inline bool isFromConstraint() const; + inline hConstraint constraint() const; +}; + +class Equation { +public: + int tag; + hEquation h; + + Expr *e; + + void Clear() {} +}; + + +class Style { +public: + int tag; + hStyle h; + + enum { + // If an entity has no style, then it will be colored according to + // whether the group that it's in is active or not, whether it's + // construction or not, and so on. + NO_STYLE = 0, + + ACTIVE_GRP = 1, + CONSTRUCTION = 2, + INACTIVE_GRP = 3, + DATUM = 4, + SOLID_EDGE = 5, + CONSTRAINT = 6, + SELECTED = 7, + HOVERED = 8, + CONTOUR_FILL = 9, + NORMALS = 10, + ANALYZE = 11, + DRAW_ERROR = 12, + DIM_SOLID = 13, + HIDDEN_EDGE = 14, + OUTLINE = 15, + + FIRST_CUSTOM = 0x100 + }; + + std::string name; + + enum class UnitsAs : uint32_t { + PIXELS = 0, + MM = 1 + }; + double width; + UnitsAs widthAs; + double textHeight; + UnitsAs textHeightAs; + enum class TextOrigin : uint32_t { + NONE = 0x00, + LEFT = 0x01, + RIGHT = 0x02, + BOT = 0x04, + TOP = 0x08 + }; + TextOrigin textOrigin; + double textAngle; + RgbaColor color; + bool filled; + RgbaColor fillColor; + bool visible; + bool exportable; + StipplePattern stippleType; + double stippleScale; + int zIndex; + + // The default styles, for entities that don't have a style assigned yet, + // and for datums and such. + typedef struct { + hStyle h; + const char *cnfPrefix; + RgbaColor color; + double width; + int zIndex; + } Default; + static const Default Defaults[]; + + static std::string CnfColor(const std::string &prefix); + static std::string CnfWidth(const std::string &prefix); + static std::string CnfTextHeight(const std::string &prefix); + static std::string CnfPrefixToName(const std::string &prefix); + + static void CreateAllDefaultStyles(); + static void CreateDefaultStyle(hStyle h); + static void FillDefaultStyle(Style *s, const Default *d = NULL, bool factory = false); + static void FreezeDefaultStyles(); + static void LoadFactoryDefaults(); + + static void AssignSelectionToStyle(uint32_t v); + static uint32_t CreateCustomStyle(bool rememberForUndo = true); + + static RgbaColor RewriteColor(RgbaColor rgb); + + static Style *Get(hStyle hs); + static RgbaColor Color(hStyle hs, bool forExport=false); + static RgbaColor Color(int hs, bool forExport=false); + static RgbaColor FillColor(hStyle hs, bool forExport=false); + static double Width(hStyle hs); + static double Width(int hs); + static double WidthMm(int hs); + static double TextHeight(hStyle hs); + static double DefaultTextHeight(); + static Canvas::Stroke Stroke(hStyle hs); + static Canvas::Stroke Stroke(int hs); + static bool Exportable(int hs); + static hStyle ForEntity(hEntity he); + static StipplePattern PatternType(hStyle hs); + static double StippleScaleMm(hStyle hs); + + std::string DescriptionString() const; + + void Clear() {} +}; + + +inline hEntity hGroup::entity(int i) const + { hEntity r; r.v = 0x80000000 | (v << 16) | (uint32_t)i; return r; } +inline hParam hGroup::param(int i) const + { hParam r; r.v = 0x80000000 | (v << 16) | (uint32_t)i; return r; } +inline hEquation hGroup::equation(int i) const + { hEquation r; r.v = (v << 16) | 0x80000000 | (uint32_t)i; return r; } + +inline bool hRequest::IsFromReferences() const { + if(v == Request::HREQUEST_REFERENCE_XY.v) return true; + if(v == Request::HREQUEST_REFERENCE_YZ.v) return true; + if(v == Request::HREQUEST_REFERENCE_ZX.v) return true; + return false; +} +inline hEntity hRequest::entity(int i) const + { hEntity r; r.v = (v << 16) | (uint32_t)i; return r; } +inline hParam hRequest::param(int i) const + { hParam r; r.v = (v << 16) | (uint32_t)i; return r; } + +inline bool hEntity::isFromRequest() const + { if(v & 0x80000000) return false; else return true; } +inline hRequest hEntity::request() const + { hRequest r; r.v = (v >> 16); return r; } +inline hGroup hEntity::group() const + { hGroup r; r.v = (v >> 16) & 0x3fff; return r; } +inline hEquation hEntity::equation(int i) const + { hEquation r; r.v = v | 0x40000000 | (uint32_t)i; return r; } + +inline hRequest hParam::request() const + { hRequest r; r.v = (v >> 16); return r; } + + +inline hEquation hConstraint::equation(int i) const + { hEquation r; r.v = (v << 16) | (uint32_t)i; return r; } +inline hParam hConstraint::param(int i) const + { hParam r; r.v = v | 0x40000000 | (uint32_t)i; return r; } + +inline bool hEquation::isFromConstraint() const + { if(v & 0xc0000000) return false; else return true; } +inline hConstraint hEquation::constraint() const + { hConstraint r; r.v = (v >> 16); return r; } + +// The format for entities stored on the clipboard. +class ClipboardRequest { +public: + Request::Type type; + int extraPoints; + hStyle style; + std::string str; + std::string font; + Platform::Path file; + bool construction; + + Vector point[MAX_POINTS_IN_ENTITY]; + double distance; + + hEntity oldEnt; + hEntity oldPointEnt[MAX_POINTS_IN_ENTITY]; + hRequest newReq; +}; + +#endif diff --git a/ThirdParty/libslvs/solvespace.h b/ThirdParty/libslvs/solvespace.h new file mode 100644 index 0000000000..c04e05d063 --- /dev/null +++ b/ThirdParty/libslvs/solvespace.h @@ -0,0 +1,935 @@ +//----------------------------------------------------------------------------- +// All declarations not grouped specially elsewhere. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- + +#ifndef __SOLVESPACE_H +#define __SOLVESPACE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// We declare these in advance instead of simply using FT_Library +// (defined as typedef FT_LibraryRec_* FT_Library) because including +// freetype.h invokes indescribable horrors and we would like to avoid +// doing that every time we include solvespace.h. + +#if FULL_LIB_JJS +struct FT_LibraryRec_; +struct FT_FaceRec_; +#endif + +typedef struct _cairo cairo_t; + +// The few floating-point equality comparisons in SolveSpace have been +// carefully considered, so we disable the -Wfloat-equal warning for them +#ifdef __clang__ +# define EXACT(expr) \ + (_Pragma("clang diagnostic push") \ + _Pragma("clang diagnostic ignored \"-Wfloat-equal\"") \ + (expr) \ + _Pragma("clang diagnostic pop")) +#else +# define EXACT(expr) (expr) +#endif + +// Debugging functions +#if defined(__GNUC__) +#define ssassert(condition, message) \ + do { \ + if(__builtin_expect((condition), true) == false) { \ + SolveSpace::assert_failure(__FILE__, __LINE__, __func__, #condition, message); \ + __builtin_unreachable(); \ + } \ + } while(0) +#else +#define ssassert(condition, message) \ + do { \ + if((condition) == false) { \ + SolveSpace::assert_failure(__FILE__, __LINE__, __func__, #condition, message); \ + abort(); \ + } \ + } while(0) +#endif + +#ifndef isnan +# define isnan(x) (((x) != (x)) || (x > 1e11) || (x < -1e11)) +#endif + +namespace SolveSpace { + +using std::min; +using std::max; +using std::swap; + +#if defined(__GNUC__) +__attribute__((noreturn)) +#endif +void assert_failure(const char *file, unsigned line, const char *function, + const char *condition, const char *message); + +#if defined(__GNUC__) +__attribute__((__format__ (__printf__, 1, 2))) +#endif +std::string ssprintf(const char *fmt, ...); + +#if FULL_LIB_JJS +inline int WRAP(int v, int n) { + // Clamp it to the range [0, n) + while(v >= n) v -= n; + while(v < 0) v += n; + return v; +} +inline double WRAP_NOT_0(double v, double n) { + // Clamp it to the range (0, n] + while(v > n) v -= n; + while(v <= 0) v += n; + return v; +} +inline double WRAP_SYMMETRIC(double v, double n) { + // Clamp it to the range (-n/2, n/2] + while(v > n/2) v -= n; + while(v <= -n/2) v += n; + return v; +} + +#endif +// Why is this faster than the library function? +inline double ffabs(double v) { return (v > 0) ? v : (-v); } + +#if FULL_LIB_JJS +#define CO(v) (v).x, (v).y, (v).z +#endif + +#define ANGLE_COS_EPS (1e-6) +#define LENGTH_EPS (1e-6) +#define VERY_POSITIVE (1e10) +#define VERY_NEGATIVE (-1e10) + +#if FULL_LIB_JJS +inline double Random(double vmax) { + return (vmax*rand()) / RAND_MAX; +} +#endif + +class Expr; +class ExprVector; +class ExprQuaternion; +class RgbaColor; + +#if FULL_LIB_JJS +enum class Command : uint32_t; +enum class ContextCommand : uint32_t; +#endif + +//================ +// From the platform-specific code. + +#include "platform/platform.h" + +#if FULL_LIB_JJS +const size_t MAX_RECENT = 8; +extern Platform::Path RecentFile[MAX_RECENT]; +void RefreshRecentMenus(); + +enum DialogChoice { DIALOG_YES = 1, DIALOG_NO = -1, DIALOG_CANCEL = 0 }; +DialogChoice SaveFileYesNoCancel(); +DialogChoice LoadAutosaveYesNo(); +DialogChoice LocateImportedFileYesNoCancel(const Platform::Path &filename, + bool canCancel); + +#define AUTOSAVE_EXT "slvs~" + +enum class Unit : uint32_t { + MM = 0, + INCHES +}; + +#endif + +#if FULL_LIB_JJS +struct FileFilter; + +bool GetSaveFile(Platform::Path *filename, const std::string &defExtension, + const FileFilter filters[]); +bool GetOpenFile(Platform::Path *filename, const std::string &defExtension, + const FileFilter filters[]); +std::vector GetFontFiles(); + +void OpenWebsite(const char *url); + +void RefreshLocale(); + +void CheckMenuByCmd(Command id, bool checked); +void RadioMenuByCmd(Command id, bool selected); +void EnableMenuByCmd(Command id, bool enabled); + +void ShowGraphicsEditControl(int x, int y, int fontHeight, int minWidthChars, + const std::string &str); +void HideGraphicsEditControl(); +bool GraphicsEditControlIsVisible(); +void ShowTextEditControl(int x, int y, const std::string &str); +void HideTextEditControl(); +bool TextEditControlIsVisible(); +void MoveTextScrollbarTo(int pos, int maxPos, int page); + +void AddContextMenuItem(const char *legend, ContextCommand id); +void CreateContextSubmenu(); +ContextCommand ShowContextMenu(); + +void ShowTextWindow(bool visible); +void InvalidateText(); +void InvalidateGraphics(); +void PaintGraphics(); +void ToggleFullScreen(); +bool FullScreenIsActive(); +void GetGraphicsWindowSize(int *w, int *h); +void GetTextWindowSize(int *w, int *h); +double GetScreenDpi(); +int64_t GetMilliseconds(); + +#endif + +void dbp(const char *str, ...); +#if FULL_LIB_JJS +#define DBPTRI(tri) \ + dbp("tri: (%.3f %.3f %.3f) (%.3f %.3f %.3f) (%.3f %.3f %.3f)", \ + CO((tri).a), CO((tri).b), CO((tri).c)) + +void SetCurrentFilename(const Platform::Path &filename); +void SetMousePointerToHand(bool yes); + +#endif +void DoMessageBox(const char *str, int rows, int cols, bool error); +#if FULL_LIB_JJS +void SetTimerFor(int milliseconds); +void SetAutosaveTimerFor(int minutes); +void ScheduleLater(); +void ExitNow(); + +#endif + +void CnfFreezeInt(uint32_t val, const std::string &name); +#if FULL_LIB_JJS +void CnfFreezeFloat(float val, const std::string &name); +void CnfFreezeString(const std::string &val, const std::string &name); +std::string CnfThawString(const std::string &val, const std::string &name); + +#endif +uint32_t CnfThawInt(uint32_t val, const std::string &name); +#if FULL_LIB_JJS +float CnfThawFloat(float val, const std::string &name); +#endif + +std::vector InitPlatform(int argc, char **argv); + +void *AllocTemporary(size_t n); +void FreeTemporary(void *p); +void FreeAllTemporary(); +void *MemAlloc(size_t n); +void MemFree(void *p); + +#if FULL_LIB_JJS +void vl(); // debug function to validate heaps +#endif + +#include "resource.h" + +// End of platform-specific functions +//================ + +template +struct CompareHandle { + bool operator()(T lhs, T rhs) const { return lhs.v < rhs.v; } +}; + +template +using handle_map = std::map>; + + +class Group; +class SSurface; +#include "dsc.h" +#include "polygon.h" +#include "srf/surface.h" +#include "render/render.h" + +class Entity; +class hEntity; +class Param; +class hParam; +typedef IdList EntityList; +typedef IdList ParamList; + +enum class SolveResult : uint32_t { + OKAY = 0, + DIDNT_CONVERGE = 10, + REDUNDANT_OKAY = 11, + REDUNDANT_DIDNT_CONVERGE = 12, + TOO_MANY_UNKNOWNS = 20 +}; + + +#include "sketch.h" +#if FULL_LIB_JJS +#include "ui.h" +#endif +#include "expr.h" + +#if FULL_LIB_JJS + +// Utility functions that are provided in the platform-independent code. +class utf8_iterator : std::iterator { + const char *p, *n; +public: + utf8_iterator(const char *p) : p(p), n(NULL) {} + bool operator==(const utf8_iterator &i) const { return p==i.p; } + bool operator!=(const utf8_iterator &i) const { return p!=i.p; } + ptrdiff_t operator- (const utf8_iterator &i) const { return p -i.p; } + utf8_iterator& operator++() { **this; p=n; n=NULL; return *this; } + utf8_iterator operator++(int) { utf8_iterator t(*this); operator++(); return t; } + char32_t operator*(); +}; + + +class ReadUTF8 { + const std::string &str; +public: + ReadUTF8(const std::string &str) : str(str) {} + utf8_iterator begin() const { return utf8_iterator(&str[0]); } + utf8_iterator end() const { return utf8_iterator(&str[str.length()]); } +}; +#endif + +#define arraylen(x) (sizeof((x))/sizeof((x)[0])) +#define PI (3.1415926535897931) +void MakeMatrix(double *mat, double a11, double a12, double a13, double a14, + double a21, double a22, double a23, double a24, + double a31, double a32, double a33, double a34, + double a41, double a42, double a43, double a44); +void MultMatrix(double *mata, double *matb, double *matr); + +std::string MakeAcceleratorLabel(int accel); +void Message(const char *str, ...); +void Error(const char *str, ...); +void CnfFreezeBool(bool v, const std::string &name); +void CnfFreezeColor(RgbaColor v, const std::string &name); +bool CnfThawBool(bool v, const std::string &name); +RgbaColor CnfThawColor(RgbaColor v, const std::string &name); + +class System { +public: + enum { MAX_UNKNOWNS = 1024 }; + + EntityList entity; + ParamList param; + IdList eq; + + // A list of parameters that are being dragged; these are the ones that + // we should put as close as possible to their initial positions. + List dragged; + + enum { + // In general, the tag indicates the subsys that a variable/equation + // has been assigned to; these are exceptions for variables: + VAR_SUBSTITUTED = 10000, + VAR_DOF_TEST = 10001, + // and for equations: + EQ_SUBSTITUTED = 20000 + }; + + // The system Jacobian matrix + struct { + // The corresponding equation for each row + hEquation eq[MAX_UNKNOWNS]; + + // The corresponding parameter for each column + hParam param[MAX_UNKNOWNS]; + + // We're solving AX = B + int m, n; + struct { + Expr *sym[MAX_UNKNOWNS][MAX_UNKNOWNS]; + double num[MAX_UNKNOWNS][MAX_UNKNOWNS]; + } A; + + double scale[MAX_UNKNOWNS]; + + // Some helpers for the least squares solve + double AAt[MAX_UNKNOWNS][MAX_UNKNOWNS]; + double Z[MAX_UNKNOWNS]; + + double X[MAX_UNKNOWNS]; + + struct { + Expr *sym[MAX_UNKNOWNS]; + double num[MAX_UNKNOWNS]; + } B; + } mat; + + static const double RANK_MAG_TOLERANCE, CONVERGE_TOLERANCE; + int CalculateRank(); + bool TestRank(); + static bool SolveLinearSystem(double X[], double A[][MAX_UNKNOWNS], + double B[], int N); + bool SolveLeastSquares(); + + bool WriteJacobian(int tag); + void EvalJacobian(); + + void WriteEquationsExceptFor(hConstraint hc, Group *g); + void FindWhichToRemoveToFixJacobian(Group *g, List *bad, bool forceDofCheck); + void SolveBySubstitution(); + + bool IsDragged(hParam p); + + bool NewtonSolve(int tag); + + void MarkParamsFree(bool findFree); + int CalculateDof(); + + SolveResult Solve(Group *g, int *dof, List *bad, + bool andFindBad, bool andFindFree, bool forceDofCheck = false); + + SolveResult SolveRank(Group *g, int *dof, List *bad, + bool andFindBad, bool andFindFree, bool forceDofCheck = false); + + void Clear(); +}; + +#if FULL_LIB_JJS +#include "ttf.h" + +class StepFileWriter { +public: + void ExportSurfacesTo(const Platform::Path &filename); + void WriteHeader(); + void WriteProductHeader(); + int ExportCurve(SBezier *sb); + int ExportCurveLoop(SBezierLoop *loop, bool inner); + void ExportSurface(SSurface *ss, SBezierList *sbl); + void WriteWireframe(); + void WriteFooter(); + + List curves; + List advancedFaces; + FILE *f; + int id; +}; + +class VectorFileWriter { +protected: + Vector u, v, n, origin; + double cameraTan, scale; + +public: + FILE *f; + Platform::Path filename; + Vector ptMin, ptMax; + + static double MmToPts(double mm); + + static VectorFileWriter *ForFile(const Platform::Path &filename); + + void SetModelviewProjection(const Vector &u, const Vector &v, const Vector &n, + const Vector &origin, double cameraTan, double scale); + Vector Transform(Vector &pos) const; + + void OutputLinesAndMesh(SBezierLoopSetSet *sblss, SMesh *sm); + + void BezierAsPwl(SBezier *sb); + void BezierAsNonrationalCubic(SBezier *sb, int depth=0); + + virtual void StartPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) = 0; + virtual void FinishPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) = 0; + virtual void Bezier(SBezier *sb) = 0; + virtual void Triangle(STriangle *tr) = 0; + virtual bool OutputConstraints(IdList *) { return false; } + virtual void StartFile() = 0; + virtual void FinishAndCloseFile() = 0; + virtual bool HasCanvasSize() const = 0; + virtual bool CanOutputMesh() const = 0; +}; +class DxfFileWriter : public VectorFileWriter { +public: + struct BezierPath { + std::vector beziers; + }; + + std::vector paths; + IdList *constraint; + + static const char *lineTypeName(StipplePattern stippleType); + + bool OutputConstraints(IdList *constraint) override; + + void StartPath( RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void FinishPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void Triangle(STriangle *tr) override; + void Bezier(SBezier *sb) override; + void StartFile() override; + void FinishAndCloseFile() override; + bool HasCanvasSize() const override { return false; } + bool CanOutputMesh() const override { return false; } + bool NeedToOutput(Constraint *c); +}; +class EpsFileWriter : public VectorFileWriter { +public: + Vector prevPt; + void MaybeMoveTo(Vector s, Vector f); + + void StartPath( RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void FinishPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void Triangle(STriangle *tr) override; + void Bezier(SBezier *sb) override; + void StartFile() override; + void FinishAndCloseFile() override; + bool HasCanvasSize() const override { return true; } + bool CanOutputMesh() const override { return true; } +}; +class PdfFileWriter : public VectorFileWriter { +public: + uint32_t xref[10]; + uint32_t bodyStart; + Vector prevPt; + void MaybeMoveTo(Vector s, Vector f); + + void StartPath( RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void FinishPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void Triangle(STriangle *tr) override; + void Bezier(SBezier *sb) override; + void StartFile() override; + void FinishAndCloseFile() override; + bool HasCanvasSize() const override { return true; } + bool CanOutputMesh() const override { return true; } +}; +class SvgFileWriter : public VectorFileWriter { +public: + Vector prevPt; + void MaybeMoveTo(Vector s, Vector f); + + void StartPath( RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void FinishPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void Triangle(STriangle *tr) override; + void Bezier(SBezier *sb) override; + void StartFile() override; + void FinishAndCloseFile() override; + bool HasCanvasSize() const override { return true; } + bool CanOutputMesh() const override { return true; } +}; +class HpglFileWriter : public VectorFileWriter { +public: + static double MmToHpglUnits(double mm); + void StartPath( RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void FinishPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void Triangle(STriangle *tr) override; + void Bezier(SBezier *sb) override; + void StartFile() override; + void FinishAndCloseFile() override; + bool HasCanvasSize() const override { return false; } + bool CanOutputMesh() const override { return false; } +}; +class Step2dFileWriter : public VectorFileWriter { + StepFileWriter sfw; + void StartPath( RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void FinishPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void Triangle(STriangle *tr) override; + void Bezier(SBezier *sb) override; + void StartFile() override; + void FinishAndCloseFile() override; + bool HasCanvasSize() const override { return false; } + bool CanOutputMesh() const override { return false; } +}; +class GCodeFileWriter : public VectorFileWriter { +public: + SEdgeList sel; + void StartPath( RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void FinishPath(RgbaColor strokeRgb, double lineWidth, + bool filled, RgbaColor fillRgb, hStyle hs) override; + void Triangle(STriangle *tr) override; + void Bezier(SBezier *sb) override; + void StartFile() override; + void FinishAndCloseFile() override; + bool HasCanvasSize() const override { return false; } + bool CanOutputMesh() const override { return false; } +}; +#endif + +#ifdef LIBRARY +# define ENTITY EntityBase +# define CONSTRAINT ConstraintBase +#else +# define ENTITY Entity +# define CONSTRAINT Constraint +#endif +class Sketch { +public: + // These are user-editable, and define the sketch. + IdList group; + List
groupOrder; + IdList constraint; + IdList request; + IdList style; + + // These are generated from the above. + IdList entity; + IdList param; + + inline CONSTRAINT *GetConstraint(hConstraint h) + { return constraint.FindById(h); } + inline ENTITY *GetEntity (hEntity h) { return entity. FindById(h); } + inline Param *GetParam (hParam h) { return param. FindById(h); } + inline Request *GetRequest(hRequest h) { return request.FindById(h); } + inline Group *GetGroup (hGroup h) { return group. FindById(h); } + // Styles are handled a bit differently. + + void Clear(); + + BBox CalculateEntityBBox(bool includingInvisible); + Group *GetRunningMeshGroupFor(hGroup h); +}; +#undef ENTITY +#undef CONSTRAINT + +#if FULL_LIB_JJS + +class SolveSpaceUI { +public: + TextWindow *pTW; + TextWindow &TW; + GraphicsWindow GW; + + // The state for undo/redo + typedef struct { + IdList group; + List
groupOrder; + IdList request; + IdList constraint; + IdList param; + IdList style; + hGroup activeGroup; + + void Clear() { + group.Clear(); + request.Clear(); + constraint.Clear(); + param.Clear(); + style.Clear(); + } + } UndoState; + enum { MAX_UNDO = 16 }; + typedef struct { + UndoState d[MAX_UNDO]; + int cnt; + int write; + } UndoStack; + UndoStack undo; + UndoStack redo; + + std::map, Platform::PathLess> images; + bool ReloadLinkedImage(const Platform::Path &saveFile, Platform::Path *filename, + bool canCancel); + + void UndoEnableMenus(); + void UndoRemember(); + void UndoUndo(); + void UndoRedo(); + void PushFromCurrentOnto(UndoStack *uk); + void PopOntoCurrentFrom(UndoStack *uk); + void UndoClearState(UndoState *ut); + void UndoClearStack(UndoStack *uk); + + // Little bits of extra configuration state + enum { MODEL_COLORS = 8 }; + RgbaColor modelColor[MODEL_COLORS]; + Vector lightDir[2]; + double lightIntensity[2]; + double ambientIntensity; + double chordTol; + double chordTolCalculated; + int maxSegments; + double exportChordTol; + int exportMaxSegments; + double cameraTangent; + float gridSpacing; + float exportScale; + float exportOffset; + bool fixExportColors; + bool drawBackFaces; + bool showContourAreas; + bool checkClosedContour; + bool showToolbar; + Platform::Path screenshotFile; + RgbaColor backgroundColor; + bool exportShadedTriangles; + bool exportPwlCurves; + bool exportCanvasSizeAuto; + bool exportMode; + struct { + float left; + float right; + float bottom; + float top; + } exportMargin; + struct { + float width; + float height; + float dx; + float dy; + } exportCanvas; + struct { + float depth; + int passes; + float feed; + float plungeFeed; + } gCode; + + Unit viewUnits; + int afterDecimalMm; + int afterDecimalInch; + int autosaveInterval; // in minutes + + std::string MmToString(double v); + double ExprToMm(Expr *e); + double StringToMm(const std::string &s); + const char *UnitName(); + double MmPerUnit(); + int UnitDigitsAfterDecimal(); + void SetUnitDigitsAfterDecimal(int v); + double ChordTolMm(); + double ExportChordTolMm(); + int GetMaxSegments(); + bool usePerspectiveProj; + double CameraTangent(); + + // Some stuff relating to the tangent arcs created non-parametrically + // as special requests. + double tangentArcRadius; + bool tangentArcManual; + bool tangentArcDeleteOld; + + // The platform-dependent code calls this before entering the msg loop + void Init(); + bool Load(const Platform::Path &filename); + void Exit(); + + // File load/save routines, including the additional files that get + // loaded when we have link groups. + FILE *fh; + void AfterNewFile(); + static void RemoveFromRecentList(const Platform::Path &filename); + static void AddToRecentList(const Platform::Path &filename); + Platform::Path saveFile; + bool fileLoadError; + bool unsaved; + typedef struct { + char type; + const char *desc; + char fmt; + void *ptr; + } SaveTable; + static const SaveTable SAVED[]; + void SaveUsingTable(const Platform::Path &filename, int type); + void LoadUsingTable(const Platform::Path &filename, char *key, char *val); + struct { + Group g; + Request r; + Entity e; + Param p; + Constraint c; + Style s; + } sv; + static void MenuFile(Command id); + bool Autosave(); + void RemoveAutosave(); + bool GetFilenameAndSave(bool saveAs); + bool OkayToStartNewFile(); + hGroup CreateDefaultDrawingGroup(); + void UpdateWindowTitle(); + void ClearExisting(); + void NewFile(); + bool SaveToFile(const Platform::Path &filename); + bool LoadAutosaveFor(const Platform::Path &filename); + bool LoadFromFile(const Platform::Path &filename, bool canCancel = false); + void UpgradeLegacyData(); + bool LoadEntitiesFromFile(const Platform::Path &filename, EntityList *le, + SMesh *m, SShell *sh); + bool ReloadAllLinked(const Platform::Path &filename, bool canCancel = false); + // And the various export options + void ExportAsPngTo(const Platform::Path &filename); + void ExportMeshTo(const Platform::Path &filename); + void ExportMeshAsStlTo(FILE *f, SMesh *sm); + void ExportMeshAsObjTo(FILE *fObj, FILE *fMtl, SMesh *sm); + void ExportMeshAsThreeJsTo(FILE *f, const Platform::Path &filename, + SMesh *sm, SOutlineList *sol); + void ExportViewOrWireframeTo(const Platform::Path &filename, bool exportWireframe); + void ExportSectionTo(const Platform::Path &filename); + void ExportWireframeCurves(SEdgeList *sel, SBezierList *sbl, + VectorFileWriter *out); + void ExportLinesAndMesh(SEdgeList *sel, SBezierList *sbl, SMesh *sm, + Vector u, Vector v, + Vector n, Vector origin, + double cameraTan, + VectorFileWriter *out); + + static void MenuAnalyze(Command id); + + // Additional display stuff + struct { + SContour path; + hEntity point; + } traced; + SEdgeList nakedEdges; + struct { + bool draw; + Vector ptA; + Vector ptB; + } extraLine; + struct { + bool draw, showOrigin; + Vector pt, u, v; + } justExportedInfo; + struct { + bool draw; + bool dirty; + Vector position; + } centerOfMass; + + class Clipboard { + public: + List r; + List c; + + void Clear(); + bool ContainsEntity(hEntity old); + hEntity NewEntityFor(hEntity old); + }; + Clipboard clipboard; + + void MarkGroupDirty(hGroup hg, bool onlyThis = false); + void MarkGroupDirtyByEntity(hEntity he); + + // Consistency checking on the sketch: stuff with missing dependencies + // will get deleted automatically. + struct { + int requests; + int groups; + int constraints; + int nonTrivialConstraints; + } deleted; + bool GroupExists(hGroup hg); + bool PruneOrphans(); + bool EntityExists(hEntity he); + bool GroupsInOrder(hGroup before, hGroup after); + bool PruneGroups(hGroup hg); + bool PruneRequests(hGroup hg); + bool PruneConstraints(hGroup hg); + static void ShowNakedEdges(bool reportOnlyWhenNotOkay); + + enum class Generate : uint32_t { + DIRTY, + ALL, + REGEN, + UNTIL_ACTIVE, + }; + + void GenerateAll(Generate type = Generate::DIRTY, bool andFindFree = false, + bool genForBBox = false); + void SolveGroup(hGroup hg, bool andFindFree); + void SolveGroupAndReport(hGroup hg, bool andFindFree); + SolveResult TestRankForGroup(hGroup hg); + void WriteEqSystemForGroup(hGroup hg); + void MarkDraggedParams(); + void ForceReferences(); + void UpdateCenterOfMass(); + + bool ActiveGroupsOkay(); + + // The system to be solved. + System *pSys; + System &sys; + + // All the TrueType fonts in memory + TtfFontList fonts; + + // Everything has been pruned, so we know there's no dangling references + // to entities that don't exist. Before that, we mustn't try to display + // the sketch! + bool allConsistent; + + struct { + bool scheduled; + bool showTW; + bool generateAll; + } later; + void ScheduleShowTW(); + void ScheduleGenerateAll(); + void DoLater(); + + static void MenuHelp(Command id); + + void Clear(); + + // We allocate TW and sys on the heap to work around an MSVC problem + // where it puts zero-initialized global data in the binary (~30M of zeroes) + // in release builds. + SolveSpaceUI() + : pTW(new TextWindow({})), TW(*pTW), + pSys(new System({})), sys(*pSys) {} + + ~SolveSpaceUI() { + delete pTW; + delete pSys; + } +}; +void ImportDxf(const Platform::Path &file); +void ImportDwg(const Platform::Path &file); + +extern SolveSpaceUI SS; +#endif +extern Sketch SK; + +} + +#ifndef __OBJC__ +using namespace SolveSpace; +#endif + +#endif diff --git a/ThirdParty/libslvs/srf/surface.h b/ThirdParty/libslvs/srf/surface.h new file mode 100644 index 0000000000..98ad820873 --- /dev/null +++ b/ThirdParty/libslvs/srf/surface.h @@ -0,0 +1,429 @@ +//----------------------------------------------------------------------------- +// Functions relating to rational polynomial surfaces, which are trimmed by +// curves (either rational polynomial curves, or piecewise linear +// approximations to curves of intersection that can't be represented +// exactly in ratpoly form), and assembled into watertight shells. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- + +#ifndef __SURFACE_H +#define __SURFACE_H + +// Utility functions, Bernstein polynomials of order 1-3 and their derivatives. +double Bernstein(int k, int deg, double t); +double BernsteinDerivative(int k, int deg, double t); + +class SBezierList; +class SSurface; +class SCurvePt; + +// Utility data structure, a two-dimensional BSP to accelerate polygon +// operations. +class SBspUv { +public: + Point2d a, b; + + SBspUv *pos; + SBspUv *neg; + + SBspUv *more; + + enum class Class : uint32_t { + INSIDE = 100, + OUTSIDE = 200, + EDGE_PARALLEL = 300, + EDGE_ANTIPARALLEL = 400, + EDGE_OTHER = 500 + }; + + static SBspUv *Alloc(); + static SBspUv *From(SEdgeList *el, SSurface *srf); + + void ScalePoints(Point2d *pt, Point2d *a, Point2d *b, SSurface *srf) const; + double ScaledSignedDistanceToLine(Point2d pt, Point2d a, Point2d b, + SSurface *srf) const; + double ScaledDistanceToLine(Point2d pt, Point2d a, Point2d b, bool asSegment, + SSurface *srf) const; + + void InsertEdge(Point2d a, Point2d b, SSurface *srf); + static SBspUv *InsertOrCreateEdge(SBspUv *where, Point2d ea, Point2d eb, SSurface *srf); + Class ClassifyPoint(Point2d p, Point2d eb, SSurface *srf) const; + Class ClassifyEdge(Point2d ea, Point2d eb, SSurface *srf) const; + double MinimumDistanceToEdge(Point2d p, SSurface *srf) const; +}; + +// Now the data structures to represent a shell of trimmed rational polynomial +// surfaces. + +class SShell; + +class hSSurface { +public: + uint32_t v; +}; + +class hSCurve { +public: + uint32_t v; +}; + +// Stuff for rational polynomial curves, of degree one to three. These are +// our inputs, and are also calculated for certain exact surface-surface +// intersections. +class SBezier { +public: + int tag; + int auxA, auxB; + + int deg; + Vector ctrl[4]; + double weight[4]; + uint32_t entity; + + Vector PointAt(double t) const; + Vector TangentAt(double t) const; + void ClosestPointTo(Vector p, double *t, bool mustConverge=true) const; + void SplitAt(double t, SBezier *bef, SBezier *aft) const; + bool PointOnThisAndCurve(const SBezier *sbb, Vector *p) const; + + Vector Start() const; + Vector Finish() const; + bool Equals(SBezier *b) const; + void MakePwlInto(SEdgeList *sel, double chordTol=0) const; + void MakePwlInto(List *l, double chordTol=0) const; + void MakePwlInto(SContour *sc, double chordTol=0) const; + void MakePwlInto(List *l, double chordTol=0) const; + void MakePwlWorker(List *l, double ta, double tb, double chordTol) const; + void MakePwlInitialWorker(List *l, double ta, double tb, double chordTol) const; + void MakeNonrationalCubicInto(SBezierList *bl, double tolerance, int depth = 0) const; + + void AllIntersectionsWith(const SBezier *sbb, SPointList *spl) const; + void GetBoundingProjd(Vector u, Vector orig, double *umin, double *umax) const; + void Reverse(); + + bool IsInPlane(Vector n, double d) const; + bool IsCircle(Vector axis, Vector *center, double *r) const; + bool IsRational() const; + + SBezier TransformedBy(Vector t, Quaternion q, double scale) const; + SBezier InPerspective(Vector u, Vector v, Vector n, + Vector origin, double cameraTan) const; + void ScaleSelfBy(double s); + + static SBezier From(Vector p0, Vector p1, Vector p2, Vector p3); + static SBezier From(Vector p0, Vector p1, Vector p2); + static SBezier From(Vector p0, Vector p1); + static SBezier From(Vector4 p0, Vector4 p1, Vector4 p2, Vector4 p3); + static SBezier From(Vector4 p0, Vector4 p1, Vector4 p2); + static SBezier From(Vector4 p0, Vector4 p1); +}; + +class SBezierList { +public: + List l; + + void Clear(); + void ScaleSelfBy(double s); + void CullIdenticalBeziers(); + void AllIntersectionsWith(SBezierList *sblb, SPointList *spl) const; + bool GetPlaneContainingBeziers(Vector *p, Vector *u, Vector *v, + Vector *notCoplanarAt) const; +}; + +class SBezierLoop { +public: + int tag; + List l; + + inline void Clear() { l.Clear(); } + bool IsClosed() const; + void Reverse(); + void MakePwlInto(SContour *sc, double chordTol=0) const; + void GetBoundingProjd(Vector u, Vector orig, double *umin, double *umax) const; + + static SBezierLoop FromCurves(SBezierList *spcl, + bool *allClosed, SEdge *errorAt); +}; + +class SBezierLoopSet { +public: + List l; + Vector normal; + Vector point; + double area; + + static SBezierLoopSet From(SBezierList *spcl, SPolygon *poly, + double chordTol, + bool *allClosed, SEdge *errorAt, + SBezierList *openContours); + + void GetBoundingProjd(Vector u, Vector orig, double *umin, double *umax) const; + double SignedArea(); + void MakePwlInto(SPolygon *sp) const; + void Clear(); +}; + +class SBezierLoopSetSet { +public: + List l; + + void FindOuterFacesFrom(SBezierList *sbl, SPolygon *spxyz, SSurface *srfuv, + double chordTol, + bool *allClosed, SEdge *notClosedAt, + bool *allCoplanar, Vector *notCoplanarAt, + SBezierList *openContours); + void AddOpenPath(SBezier *sb); + void Clear(); +}; + +// Stuff for the surface trim curves: piecewise linear +class SCurvePt { +public: + int tag; + Vector p; + bool vertex; +}; + +class SCurve { +public: + hSCurve h; + + // In a Boolean, C = A op B. The curves in A and B get copied into C, and + // therefore must get new hSCurves assigned. For the curves in A and B, + // we use newH to record their new handle in C. + hSCurve newH; + enum class Source : uint32_t { + A = 100, + B = 200, + INTERSECTION = 300 + }; + Source source; + + bool isExact; + SBezier exact; + + List pts; + + hSSurface surfA; + hSSurface surfB; + + static SCurve FromTransformationOf(SCurve *a, Vector t, + Quaternion q, double scale); + SCurve MakeCopySplitAgainst(SShell *agnstA, SShell *agnstB, + SSurface *srfA, SSurface *srfB) const; + void RemoveShortSegments(SSurface *srfA, SSurface *srfB); + SSurface *GetSurfaceA(SShell *a, SShell *b) const; + SSurface *GetSurfaceB(SShell *a, SShell *b) const; + + void Clear(); +}; + +// A segment of a curve by which a surface is trimmed: indicates which curve, +// by its handle, and the starting and ending points of our segment of it. +// The vector out points out of the surface; it, the surface outer normal, +// and a tangent to the beginning of the curve are all orthogonal. +class STrimBy { +public: + hSCurve curve; + bool backwards; + // If a trim runs backwards, then start and finish still correspond to + // the actual start and finish, but they appear in reverse order in + // the referenced curve. + Vector start; + Vector finish; + + static STrimBy EntireCurve(SShell *shell, hSCurve hsc, bool backwards); +}; + +// An intersection point between a line and a surface +class SInter { +public: + int tag; + Vector p; + SSurface *srf; + Point2d pinter; + Vector surfNormal; // of the intersecting surface, at pinter + bool onEdge; // pinter is on edge of trim poly +}; + +// A rational polynomial surface in Bezier form. +class SSurface { +public: + + enum class CombineAs : uint32_t { + UNION = 10, + DIFFERENCE = 11, + INTERSECT = 12 + }; + + int tag; + hSSurface h; + + // Same as newH for the curves; record what a surface gets renamed to + // when I copy things over. + hSSurface newH; + + RgbaColor color; + uint32_t face; + + int degm, degn; + Vector ctrl[4][4]; + double weight[4][4]; + + List trim; + + // For testing whether a point (u, v) on the surface lies inside the trim + SBspUv *bsp; + SEdgeList edges; + + // For caching our initial (u, v) when doing Newton iterations to project + // a point into our surface. + Point2d cached; + + static SSurface FromExtrusionOf(SBezier *spc, Vector t0, Vector t1); + static SSurface FromRevolutionOf(SBezier *sb, Vector pt, Vector axis, + double thetas, double thetaf); + static SSurface FromPlane(Vector pt, Vector u, Vector v); + static SSurface FromTransformationOf(SSurface *a, Vector t, Quaternion q, + double scale, + bool includingTrims); + void ScaleSelfBy(double s); + + void EdgeNormalsWithinSurface(Point2d auv, Point2d buv, + Vector *pt, Vector *enin, Vector *enout, + Vector *surfn, + uint32_t auxA, + SShell *shell, SShell *sha, SShell *shb); + void FindChainAvoiding(SEdgeList *src, SEdgeList *dest, SPointList *avoid); + SSurface MakeCopyTrimAgainst(SShell *parent, SShell *a, SShell *b, + SShell *into, SSurface::CombineAs type); + void TrimFromEdgeList(SEdgeList *el, bool asUv); + void IntersectAgainst(SSurface *b, SShell *agnstA, SShell *agnstB, + SShell *into); + void AddExactIntersectionCurve(SBezier *sb, SSurface *srfB, + SShell *agnstA, SShell *agnstB, SShell *into); + + typedef struct { + int tag; + Point2d p; + } Inter; + void WeightControlPoints(); + void UnWeightControlPoints(); + void CopyRowOrCol(bool row, int this_ij, SSurface *src, int src_ij); + void BlendRowOrCol(bool row, int this_ij, SSurface *a, int a_ij, + SSurface *b, int b_ij); + double DepartureFromCoplanar() const; + void SplitInHalf(bool byU, SSurface *sa, SSurface *sb); + void AllPointsIntersecting(Vector a, Vector b, + List *l, + bool asSegment, bool trimmed, bool inclTangent); + void AllPointsIntersectingUntrimmed(Vector a, Vector b, + int *cnt, int *level, + List *l, bool asSegment, + SSurface *sorig); + + void ClosestPointTo(Vector p, Point2d *puv, bool mustConverge=true); + void ClosestPointTo(Vector p, double *u, double *v, bool mustConverge=true); + bool ClosestPointNewton(Vector p, double *u, double *v, bool mustConverge=true) const; + + bool PointIntersectingLine(Vector p0, Vector p1, double *u, double *v) const; + Vector ClosestPointOnThisAndSurface(SSurface *srf2, Vector p); + void PointOnSurfaces(SSurface *s1, SSurface *s2, double *u, double *v); + Vector PointAt(double u, double v) const; + Vector PointAt(Point2d puv) const; + void TangentsAt(double u, double v, Vector *tu, Vector *tv) const; + Vector NormalAt(Point2d puv) const; + Vector NormalAt(double u, double v) const; + bool LineEntirelyOutsideBbox(Vector a, Vector b, bool asSegment) const; + void GetAxisAlignedBounding(Vector *ptMax, Vector *ptMin) const; + bool CoincidentWithPlane(Vector n, double d) const; + bool CoincidentWith(SSurface *ss, bool sameNormal) const; + bool IsExtrusion(SBezier *of, Vector *along) const; + bool IsCylinder(Vector *axis, Vector *center, double *r, + Vector *start, Vector *finish) const; + + void TriangulateInto(SShell *shell, SMesh *sm); + + // these are intended as bitmasks, even though there's just one now + enum class MakeAs : uint32_t { + UV = 0x01, + XYZ = 0x00 + }; + void MakeTrimEdgesInto(SEdgeList *sel, MakeAs flags, SCurve *sc, STrimBy *stb); + void MakeEdgesInto(SShell *shell, SEdgeList *sel, MakeAs flags, + SShell *useCurvesFrom=NULL); + + Vector ExactSurfaceTangentAt(Vector p, SSurface *srfA, SSurface *srfB, + Vector dir); + void MakeSectionEdgesInto(SShell *shell, SEdgeList *sel, SBezierList *sbl); + void MakeClassifyingBsp(SShell *shell, SShell *useCurvesFrom); + double ChordToleranceForEdge(Vector a, Vector b) const; + void MakeTriangulationGridInto(List *l, double vs, double vf, + bool swapped) const; + Vector PointAtMaybeSwapped(double u, double v, bool swapped) const; + + void Reverse(); + void Clear(); +}; + +class SShell { +public: + IdList curve; + IdList surface; + + bool booleanFailed; + + void MakeFromExtrusionOf(SBezierLoopSet *sbls, Vector t0, Vector t1, + RgbaColor color); + void MakeFromRevolutionOf(SBezierLoopSet *sbls, Vector pt, Vector axis, + RgbaColor color, Group *group); + + void MakeFromUnionOf(SShell *a, SShell *b); + void MakeFromDifferenceOf(SShell *a, SShell *b); + void MakeFromBoolean(SShell *a, SShell *b, SSurface::CombineAs type); + void CopyCurvesSplitAgainst(bool opA, SShell *agnst, SShell *into); + void CopySurfacesTrimAgainst(SShell *sha, SShell *shb, SShell *into, SSurface::CombineAs type); + void MakeIntersectionCurvesAgainst(SShell *against, SShell *into); + void MakeClassifyingBsps(SShell *useCurvesFrom); + void AllPointsIntersecting(Vector a, Vector b, List *il, + bool asSegment, bool trimmed, bool inclTangent); + void MakeCoincidentEdgesInto(SSurface *proto, bool sameNormal, + SEdgeList *el, SShell *useCurvesFrom); + void RewriteSurfaceHandlesForCurves(SShell *a, SShell *b); + void CleanupAfterBoolean(); + + // Definitions when classifying regions of a surface; it is either inside, + // outside, or coincident (with parallel or antiparallel normal) with a + // shell. + enum class Class : uint32_t { + INSIDE = 100, + OUTSIDE = 200, + COINC_SAME = 300, + COINC_OPP = 400 + }; + static const double DOTP_TOL; + Class ClassifyRegion(Vector edge_n, Vector inter_surf_n, + Vector edge_surf_n) const; + + bool ClassifyEdge(Class *indir, Class *outdir, + Vector ea, Vector eb, + Vector p, Vector edge_n_in, + Vector edge_n_out, Vector surf_n); + + void MakeFromCopyOf(SShell *a); + void MakeFromTransformationOf(SShell *a, + Vector trans, Quaternion q, double scale); + void MakeFromAssemblyOf(SShell *a, SShell *b); + void MergeCoincidentSurfaces(); + + void TriangulateInto(SMesh *sm); + void MakeEdgesInto(SEdgeList *sel); + void MakeSectionEdgesInto(Vector n, double d, SEdgeList *sel, SBezierList *sbl); + bool IsEmpty() const; + void RemapFaces(Group *g, int remap); + void Clear(); +}; + +#endif + diff --git a/ThirdParty/libslvs/system.cpp b/ThirdParty/libslvs/system.cpp new file mode 100644 index 0000000000..e90611a9f2 --- /dev/null +++ b/ThirdParty/libslvs/system.cpp @@ -0,0 +1,587 @@ +//----------------------------------------------------------------------------- +// Once we've written our constraint equations in the symbolic algebra system, +// these routines linearize them, and solve by a modified Newton's method. +// This also contains the routines to detect non-convergence or inconsistency, +// and report diagnostics to the user. +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- +#include "solvespace.h" + +// This tolerance is used to determine whether two (linearized) constraints +// are linearly dependent. If this is too small, then we will attempt to +// solve truly inconsistent systems and fail. But if it's too large, then +// we will give up on legitimate systems like a skinny right angle triangle by +// its hypotenuse and long side. +const double System::RANK_MAG_TOLERANCE = 1e-4; + +// The solver will converge all unknowns to within this tolerance. This must +// always be much less than LENGTH_EPS, and in practice should be much less. +const double System::CONVERGE_TOLERANCE = (LENGTH_EPS/(1e2)); + +bool System::WriteJacobian(int tag) { + int a, i, j; + + j = 0; + for(a = 0; a < param.n; a++) { + if(j >= MAX_UNKNOWNS) return false; + + Param *p = &(param.elem[a]); + if(p->tag != tag) continue; + mat.param[j] = p->h; + j++; + } + mat.n = j; + + i = 0; + for(a = 0; a < eq.n; a++) { + if(i >= MAX_UNKNOWNS) return false; + + Equation *e = &(eq.elem[a]); + if(e->tag != tag) continue; + + mat.eq[i] = e->h; + Expr *f = e->e->DeepCopyWithParamsAsPointers(¶m, &(SK.param)); + f = f->FoldConstants(); + + // Hash table (61 bits) to accelerate generation of zero partials. + uint64_t scoreboard = f->ParamsUsed(); + for(j = 0; j < mat.n; j++) { + Expr *pd; + if(scoreboard & ((uint64_t)1 << (mat.param[j].v % 61)) && + f->DependsOn(mat.param[j])) + { + pd = f->PartialWrt(mat.param[j]); + pd = pd->FoldConstants(); + pd = pd->DeepCopyWithParamsAsPointers(¶m, &(SK.param)); + } else { + pd = Expr::From(0.0); + } + mat.A.sym[i][j] = pd; + } + mat.B.sym[i] = f; + i++; + } + mat.m = i; + + return true; +} + +void System::EvalJacobian() { + int i, j; + for(i = 0; i < mat.m; i++) { + for(j = 0; j < mat.n; j++) { + mat.A.num[i][j] = (mat.A.sym[i][j])->Eval(); + } + } +} + +bool System::IsDragged(hParam p) { + hParam *pp; + for(pp = dragged.First(); pp; pp = dragged.NextAfter(pp)) { + if(p.v == pp->v) return true; + } + return false; +} + +void System::SolveBySubstitution() { + int i; + for(i = 0; i < eq.n; i++) { + Equation *teq = &(eq.elem[i]); + Expr *tex = teq->e; + + if(tex->op == Expr::Op::MINUS && + tex->a->op == Expr::Op::PARAM && + tex->b->op == Expr::Op::PARAM) + { + hParam a = tex->a->parh; + hParam b = tex->b->parh; + if(!(param.FindByIdNoOops(a) && param.FindByIdNoOops(b))) { + // Don't substitute unless they're both solver params; + // otherwise it's an equation that can be solved immediately, + // or an error to flag later. + continue; + } + + if(IsDragged(a)) { + // A is being dragged, so A should stay, and B should go + hParam t = a; + a = b; + b = t; + } + + int j; + for(j = 0; j < eq.n; j++) { + Equation *req = &(eq.elem[j]); + (req->e)->Substitute(a, b); // A becomes B, B unchanged + } + for(j = 0; j < param.n; j++) { + Param *rp = &(param.elem[j]); + if(rp->substd.v == a.v) { + rp->substd = b; + } + } + Param *ptr = param.FindById(a); + ptr->tag = VAR_SUBSTITUTED; + ptr->substd = b; + + teq->tag = EQ_SUBSTITUTED; + } + } +} + +//----------------------------------------------------------------------------- +// Calculate the rank of the Jacobian matrix, by Gram-Schimdt orthogonalization +// in place. A row (~equation) is considered to be all zeros if its magnitude +// is less than the tolerance RANK_MAG_TOLERANCE. +//----------------------------------------------------------------------------- +int System::CalculateRank() { + // Actually work with magnitudes squared, not the magnitudes + double rowMag[MAX_UNKNOWNS] = {}; + double tol = RANK_MAG_TOLERANCE*RANK_MAG_TOLERANCE; + + int i, iprev, j; + int rank = 0; + + for(i = 0; i < mat.m; i++) { + // Subtract off this row's component in the direction of any + // previous rows + for(iprev = 0; iprev < i; iprev++) { + if(rowMag[iprev] <= tol) continue; // ignore zero rows + + double dot = 0; + for(j = 0; j < mat.n; j++) { + dot += (mat.A.num[iprev][j]) * (mat.A.num[i][j]); + } + for(j = 0; j < mat.n; j++) { + mat.A.num[i][j] -= (dot/rowMag[iprev])*mat.A.num[iprev][j]; + } + } + // Our row is now normal to all previous rows; calculate the + // magnitude of what's left + double mag = 0; + for(j = 0; j < mat.n; j++) { + mag += (mat.A.num[i][j]) * (mat.A.num[i][j]); + } + if(mag > tol) { + rank++; + } + rowMag[i] = mag; + } + + return rank; +} + +bool System::TestRank() { + EvalJacobian(); + return CalculateRank() == mat.m; +} + +bool System::SolveLinearSystem(double X[], double A[][MAX_UNKNOWNS], + double B[], int n) +{ + // Gaussian elimination, with partial pivoting. It's an error if the + // matrix is singular, because that means two constraints are + // equivalent. + int i, j, ip, jp, imax = 0; + double max, temp; + + for(i = 0; i < n; i++) { + // We are trying eliminate the term in column i, for rows i+1 and + // greater. First, find a pivot (between rows i and N-1). + max = 0; + for(ip = i; ip < n; ip++) { + if(ffabs(A[ip][i]) > max) { + imax = ip; + max = ffabs(A[ip][i]); + } + } + // Don't give up on a singular matrix unless it's really bad; the + // assumption code is responsible for identifying that condition, + // so we're not responsible for reporting that error. + if(ffabs(max) < 1e-20) continue; + + // Swap row imax with row i + for(jp = 0; jp < n; jp++) { + swap(A[i][jp], A[imax][jp]); + } + swap(B[i], B[imax]); + + // For rows i+1 and greater, eliminate the term in column i. + for(ip = i+1; ip < n; ip++) { + temp = A[ip][i]/A[i][i]; + + for(jp = i; jp < n; jp++) { + A[ip][jp] -= temp*(A[i][jp]); + } + B[ip] -= temp*B[i]; + } + } + + // We've put the matrix in upper triangular form, so at this point we + // can solve by back-substitution. + for(i = n - 1; i >= 0; i--) { + if(ffabs(A[i][i]) < 1e-20) continue; + + temp = B[i]; + for(j = n - 1; j > i; j--) { + temp -= X[j]*A[i][j]; + } + X[i] = temp / A[i][i]; + } + + return true; +} + +bool System::SolveLeastSquares() { + int r, c, i; + + // Scale the columns; this scale weights the parameters for the least + // squares solve, so that we can encourage the solver to make bigger + // changes in some parameters, and smaller in others. + for(c = 0; c < mat.n; c++) { + if(IsDragged(mat.param[c])) { + // It's least squares, so this parameter doesn't need to be all + // that big to get a large effect. + mat.scale[c] = 1/20.0; + } else { + mat.scale[c] = 1; + } + for(r = 0; r < mat.m; r++) { + mat.A.num[r][c] *= mat.scale[c]; + } + } + + // Write A*A' + for(r = 0; r < mat.m; r++) { + for(c = 0; c < mat.m; c++) { // yes, AAt is square + double sum = 0; + for(i = 0; i < mat.n; i++) { + sum += mat.A.num[r][i]*mat.A.num[c][i]; + } + mat.AAt[r][c] = sum; + } + } + + if(!SolveLinearSystem(mat.Z, mat.AAt, mat.B.num, mat.m)) return false; + + // And multiply that by A' to get our solution. + for(c = 0; c < mat.n; c++) { + double sum = 0; + for(i = 0; i < mat.m; i++) { + sum += mat.A.num[i][c]*mat.Z[i]; + } + mat.X[c] = sum * mat.scale[c]; + } + return true; +} + +bool System::NewtonSolve(int tag) { + + int iter = 0; + bool converged = false; + int i; + + // Evaluate the functions at our operating point. + for(i = 0; i < mat.m; i++) { + mat.B.num[i] = (mat.B.sym[i])->Eval(); + } + do { + // And evaluate the Jacobian at our initial operating point. + EvalJacobian(); + + if(!SolveLeastSquares()) break; + + // Take the Newton step; + // J(x_n) (x_{n+1} - x_n) = 0 - F(x_n) + for(i = 0; i < mat.n; i++) { + Param *p = param.FindById(mat.param[i]); + p->val -= mat.X[i]; + if(isnan(p->val)) { + // Very bad, and clearly not convergent + return false; + } + } + + // Re-evalute the functions, since the params have just changed. + for(i = 0; i < mat.m; i++) { + mat.B.num[i] = (mat.B.sym[i])->Eval(); + } + // Check for convergence + converged = true; + for(i = 0; i < mat.m; i++) { + if(isnan(mat.B.num[i])) { + return false; + } + if(ffabs(mat.B.num[i]) > CONVERGE_TOLERANCE) { + converged = false; + break; + } + } + } while(iter++ < 50 && !converged); + + return converged; +} + +void System::WriteEquationsExceptFor(hConstraint hc, Group *g) { + int i; + // Generate all the equations from constraints in this group + for(i = 0; i < SK.constraint.n; i++) { + ConstraintBase *c = &(SK.constraint.elem[i]); + if(c->group.v != g->h.v) continue; + if(c->h.v == hc.v) continue; + + if(c->HasLabel() && c->type != Constraint::Type::COMMENT && + g->allDimsReference) + { + // When all dimensions are reference, we adjust them to display + // the correct value, and then don't generate any equations. + c->ModifyToSatisfy(); + continue; + } + if(g->relaxConstraints && c->type != Constraint::Type::POINTS_COINCIDENT) { + // When the constraints are relaxed, we keep only the point- + // coincident constraints, and the constraints generated by + // the entities and groups. + continue; + } + + c->GenerateEquations(&eq); + } + // And the equations from entities + for(i = 0; i < SK.entity.n; i++) { + EntityBase *e = &(SK.entity.elem[i]); + if(e->group.v != g->h.v) continue; + + e->GenerateEquations(&eq); + } + // And from the groups themselves + g->GenerateEquations(&eq); +} + +void System::FindWhichToRemoveToFixJacobian(Group *g, List *bad, bool forceDofCheck) { + int a, i; + + for(a = 0; a < 2; a++) { + for(i = 0; i < SK.constraint.n; i++) { + ConstraintBase *c = &(SK.constraint.elem[i]); + if(c->group.v != g->h.v) continue; + if((c->type == Constraint::Type::POINTS_COINCIDENT && a == 0) || + (c->type != Constraint::Type::POINTS_COINCIDENT && a == 1)) + { + // Do the constraints in two passes: first everything but + // the point-coincident constraints, then only those + // constraints (so they appear last in the list). + continue; + } + + param.ClearTags(); + eq.Clear(); + WriteEquationsExceptFor(c->h, g); + eq.ClearTags(); + + // It's a major speedup to solve the easy ones by substitution here, + // and that doesn't break anything. + if(!forceDofCheck) { + SolveBySubstitution(); + } + + WriteJacobian(0); + EvalJacobian(); + + int rank = CalculateRank(); + if(rank == mat.m) { + // We fixed it by removing this constraint + bad->Add(&(c->h)); + } + } + } +} + +SolveResult System::Solve(Group *g, int *dof, List *bad, + bool andFindBad, bool andFindFree, bool forceDofCheck) +{ + WriteEquationsExceptFor(Constraint::NO_CONSTRAINT, g); + + int i; + bool rankOk; + +/* + dbp("%d equations", eq.n); + for(i = 0; i < eq.n; i++) { + dbp(" %.3f = %s = 0", eq.elem[i].e->Eval(), eq.elem[i].e->Print()); + } + dbp("%d parameters", param.n); + for(i = 0; i < param.n; i++) { + dbp(" param %08x at %.3f", param.elem[i].h.v, param.elem[i].val); + } */ + + // All params and equations are assigned to group zero. + param.ClearTags(); + eq.ClearTags(); + + if(!forceDofCheck) { + SolveBySubstitution(); + } + + // Before solving the big system, see if we can find any equations that + // are soluble alone. This can be a huge speedup. We don't know whether + // the system is consistent yet, but if it isn't then we'll catch that + // later. + int alone = 1; + for(i = 0; i < eq.n; i++) { + Equation *e = &(eq.elem[i]); + if(e->tag != 0) continue; + + hParam hp = e->e->ReferencedParams(¶m); + if(hp.v == Expr::NO_PARAMS.v) continue; + if(hp.v == Expr::MULTIPLE_PARAMS.v) continue; + + Param *p = param.FindById(hp); + if(p->tag != 0) continue; // let rank test catch inconsistency + + e->tag = alone; + p->tag = alone; + WriteJacobian(alone); + if(!NewtonSolve(alone)) { + // We don't do the rank test, so let's arbitrarily return + // the DIDNT_CONVERGE result here. + rankOk = true; + // Failed to converge, bail out early + goto didnt_converge; + } + alone++; + } + + // Now write the Jacobian for what's left, and do a rank test; that + // tells us if the system is inconsistently constrained. + if(!WriteJacobian(0)) { + return SolveResult::TOO_MANY_UNKNOWNS; + } + + rankOk = TestRank(); + + // And do the leftovers as one big system + if(!NewtonSolve(0)) { + goto didnt_converge; + } + + rankOk = TestRank(); + if(!rankOk) { + if(!g->allowRedundant) { + if(andFindBad) FindWhichToRemoveToFixJacobian(g, bad, forceDofCheck); + } + } else { + // This is not the full Jacobian, but any substitutions or single-eq + // solves removed one equation and one unknown, therefore no effect + // on the number of DOF. + if(dof) *dof = CalculateDof(); + MarkParamsFree(andFindFree); + } + // System solved correctly, so write the new values back in to the + // main parameter table. + for(i = 0; i < param.n; i++) { + Param *p = &(param.elem[i]); + double val; + if(p->tag == VAR_SUBSTITUTED) { + val = param.FindById(p->substd)->val; + } else { + val = p->val; + } + Param *pp = SK.GetParam(p->h); + pp->val = val; + pp->known = true; + pp->free = p->free; + } + return rankOk ? SolveResult::OKAY : SolveResult::REDUNDANT_OKAY; + +didnt_converge: + SK.constraint.ClearTags(); + for(i = 0; i < eq.n; i++) { + if(ffabs(mat.B.num[i]) > CONVERGE_TOLERANCE || isnan(mat.B.num[i])) { + // This constraint is unsatisfied. + if(!mat.eq[i].isFromConstraint()) continue; + + hConstraint hc = mat.eq[i].constraint(); + ConstraintBase *c = SK.constraint.FindByIdNoOops(hc); + if(!c) continue; + // Don't double-show constraints that generated multiple + // unsatisfied equations + if(!c->tag) { + bad->Add(&(c->h)); + c->tag = 1; + } + } + } + + return rankOk ? SolveResult::DIDNT_CONVERGE : SolveResult::REDUNDANT_DIDNT_CONVERGE; +} + +SolveResult System::SolveRank(Group *g, int *dof, List *bad, + bool andFindBad, bool andFindFree, bool forceDofCheck) +{ + WriteEquationsExceptFor(Constraint::NO_CONSTRAINT, g); + + // All params and equations are assigned to group zero. + param.ClearTags(); + eq.ClearTags(); + + if(!forceDofCheck) { + SolveBySubstitution(); + } + + // Now write the Jacobian, and do a rank test; that + // tells us if the system is inconsistently constrained. + if(!WriteJacobian(0)) { + return SolveResult::TOO_MANY_UNKNOWNS; + } + + bool rankOk = TestRank(); + if(!rankOk) { + if(!g->allowRedundant) { + if(andFindBad) FindWhichToRemoveToFixJacobian(g, bad, forceDofCheck); + } + } else { + // This is not the full Jacobian, but any substitutions or single-eq + // solves removed one equation and one unknown, therefore no effect + // on the number of DOF. + if(dof) *dof = CalculateDof(); + MarkParamsFree(andFindFree); + } + return rankOk ? SolveResult::OKAY : SolveResult::REDUNDANT_OKAY; +} + +void System::Clear() { + entity.Clear(); + param.Clear(); + eq.Clear(); + dragged.Clear(); +} + +void System::MarkParamsFree(bool find) { + // If requested, find all the free (unbound) variables. This might be + // more than the number of degrees of freedom. Don't always do this, + // because the display would get annoying and it's slow. + for(int i = 0; i < param.n; i++) { + Param *p = &(param.elem[i]); + p->free = false; + + if(find) { + if(p->tag == 0) { + p->tag = VAR_DOF_TEST; + WriteJacobian(0); + EvalJacobian(); + int rank = CalculateRank(); + if(rank == mat.m) { + p->free = true; + } + p->tag = 0; + } + } + } +} + +int System::CalculateDof() { + return mat.n - mat.m; +} + diff --git a/ThirdParty/libslvs/util.cpp b/ThirdParty/libslvs/util.cpp new file mode 100644 index 0000000000..e6ed51b32e --- /dev/null +++ b/ThirdParty/libslvs/util.cpp @@ -0,0 +1,1161 @@ +//----------------------------------------------------------------------------- +// Utility functions, mostly various kinds of vector math (working on real +// numbers, not working on quantities in the symbolic algebra system). +// +// Copyright 2008-2013 Jonathan Westhues. +//----------------------------------------------------------------------------- +#define _USE_MATH_DEFINES +#include "solvespace.h" + +#include + +std::string SolveSpace::ssprintf(const char *fmt, ...) +{ + va_list va; + + va_start(va, fmt); + int size = vsnprintf(NULL, 0, fmt, va); + ssassert(size >= 0, "vsnprintf could not encode string"); + va_end(va); + + std::string result; + result.resize(size + 1); + + va_start(va, fmt); + vsnprintf(&result[0], size + 1, fmt, va); + va_end(va); + + result.resize(size); + return result; +} +#if FULL_LIB_JJS + +char32_t utf8_iterator::operator*() +{ + const uint8_t *it = (const uint8_t*) this->p; + char32_t result = *it; + + if((result & 0x80) != 0) { + unsigned int mask = 0x40; + + do { + result <<= 6; + unsigned int c = (*++it); + mask <<= 5; + result += c - 0x80; + } while((result & mask) != 0); + + result &= mask - 1; + } + + this->n = (const char*) (it + 1); + return result; +} +int64_t SolveSpace::GetMilliseconds() +{ + auto timestamp = std::chrono::steady_clock::now().time_since_epoch(); + return std::chrono::duration_cast(timestamp).count(); +} +#endif + +void SolveSpace::MakeMatrix(double *mat, + double a11, double a12, double a13, double a14, + double a21, double a22, double a23, double a24, + double a31, double a32, double a33, double a34, + double a41, double a42, double a43, double a44) +{ + mat[ 0] = a11; + mat[ 1] = a21; + mat[ 2] = a31; + mat[ 3] = a41; + mat[ 4] = a12; + mat[ 5] = a22; + mat[ 6] = a32; + mat[ 7] = a42; + mat[ 8] = a13; + mat[ 9] = a23; + mat[10] = a33; + mat[11] = a43; + mat[12] = a14; + mat[13] = a24; + mat[14] = a34; + mat[15] = a44; +} + +void SolveSpace::MultMatrix(double *mata, double *matb, double *matr) { + for(int i = 0; i < 4; i++) { + for(int j = 0; j < 4; j++) { + double s = 0.0; + for(int k = 0; k < 4; k++) { + s += mata[k * 4 + j] * matb[i * 4 + k]; + } + matr[i * 4 + j] = s; + } + } +} + +//----------------------------------------------------------------------------- +// Word-wrap the string for our message box appropriately, and then display +// that string. +//----------------------------------------------------------------------------- +static void DoStringForMessageBox(const char *str, va_list f, bool error) +{ + char inBuf[1024*50]; + vsprintf(inBuf, str, f); + + char outBuf[1024*50]; + int i = 0, j = 0, len = 0, longestLen = 47; + int rows = 0, cols = 0; + + // Count the width of the longest line that starts with spaces; those + // are list items, that should not be split in the middle. + bool listLine = false; + while(inBuf[i]) { + if(inBuf[i] == '\r') { + // ignore these + } else if(inBuf[i] == ' ' && len == 0) { + listLine = true; + } else if(inBuf[i] == '\n') { + if(listLine) longestLen = max(longestLen, len); + len = 0; + } else { + len++; + } + i++; + } + if(listLine) longestLen = max(longestLen, len); + + // Word wrap according to our target line length longestLen. + len = 0; + i = 0; + while(inBuf[i]) { + if(inBuf[i] == '\r') { + // ignore these + } else if(inBuf[i] == '\n') { + outBuf[j++] = '\n'; + if(len == 0) rows++; + len = 0; + } else if(inBuf[i] == ' ' && len > longestLen) { + outBuf[j++] = '\n'; + len = 0; + } else { + outBuf[j++] = inBuf[i]; + // Count rows when we draw the first character; so an empty + // row doesn't end up counting. + if(len == 0) rows++; + len++; + } + cols = max(cols, len); + i++; + } + outBuf[j++] = '\0'; + + // And then display the text with our actual longest line length. + DoMessageBox(outBuf, rows, cols, error); +} +void SolveSpace::Error(const char *str, ...) +{ + va_list f; + va_start(f, str); + DoStringForMessageBox(str, f, /*error=*/true); + va_end(f); +} +void SolveSpace::Message(const char *str, ...) +{ + va_list f; + va_start(f, str); + DoStringForMessageBox(str, f, /*error=*/false); + va_end(f); +} + +void SolveSpace::CnfFreezeBool(bool v, const std::string &name) + { CnfFreezeInt(v ? 1 : 0, name); } + +void SolveSpace::CnfFreezeColor(RgbaColor v, const std::string &name) + { CnfFreezeInt(v.ToPackedInt(), name); } + +bool SolveSpace::CnfThawBool(bool v, const std::string &name) + { return CnfThawInt(v ? 1 : 0, name) != 0; } + +RgbaColor SolveSpace::CnfThawColor(RgbaColor v, const std::string &name) + { return RgbaColor::FromPackedInt(CnfThawInt(v.ToPackedInt(), name)); } + +//----------------------------------------------------------------------------- +// Solve a mostly banded matrix. In a given row, there are LEFT_OF_DIAG +// elements to the left of the diagonal element, and RIGHT_OF_DIAG elements to +// the right (so that the total band width is LEFT_OF_DIAG + RIGHT_OF_DIAG + 1). +// There also may be elements in the last two columns of any row. We solve +// without pivoting. +//----------------------------------------------------------------------------- +void BandedMatrix::Solve() { + int i, ip, j, jp; + double temp; + + // Reduce the matrix to upper triangular form. + for(i = 0; i < n; i++) { + for(ip = i+1; ip < n && ip <= (i + LEFT_OF_DIAG); ip++) { + temp = A[ip][i]/A[i][i]; + + for(jp = i; jp < (n - 2) && jp <= (i + RIGHT_OF_DIAG); jp++) { + A[ip][jp] -= temp*(A[i][jp]); + } + A[ip][n-2] -= temp*(A[i][n-2]); + A[ip][n-1] -= temp*(A[i][n-1]); + + B[ip] -= temp*B[i]; + } + } + + // And back-substitute. + for(i = n - 1; i >= 0; i--) { + temp = B[i]; + + if(i < n-1) temp -= X[n-1]*A[i][n-1]; + if(i < n-2) temp -= X[n-2]*A[i][n-2]; + + for(j = min(n - 3, i + RIGHT_OF_DIAG); j > i; j--) { + temp -= X[j]*A[i][j]; + } + X[i] = temp / A[i][i]; + } +} + +const Quaternion Quaternion::IDENTITY = { 1, 0, 0, 0 }; + +Quaternion Quaternion::From(double w, double vx, double vy, double vz) { + Quaternion q; + q.w = w; + q.vx = vx; + q.vy = vy; + q.vz = vz; + return q; +} + +Quaternion Quaternion::From(hParam w, hParam vx, hParam vy, hParam vz) { + Quaternion q; + q.w = SK.GetParam(w )->val; + q.vx = SK.GetParam(vx)->val; + q.vy = SK.GetParam(vy)->val; + q.vz = SK.GetParam(vz)->val; + return q; +} + +Quaternion Quaternion::From(Vector axis, double dtheta) { + Quaternion q; + double c = cos(dtheta / 2), s = sin(dtheta / 2); + axis = axis.WithMagnitude(s); + q.w = c; + q.vx = axis.x; + q.vy = axis.y; + q.vz = axis.z; + return q; +} + +Quaternion Quaternion::From(Vector u, Vector v) +{ + Vector n = u.Cross(v); + + Quaternion q; + double s, tr = 1 + u.x + v.y + n.z; + if(tr > 1e-4) { + s = 2*sqrt(tr); + q.w = s/4; + q.vx = (v.z - n.y)/s; + q.vy = (n.x - u.z)/s; + q.vz = (u.y - v.x)/s; + } else { + if(u.x > v.y && u.x > n.z) { + s = 2*sqrt(1 + u.x - v.y - n.z); + q.w = (v.z - n.y)/s; + q.vx = s/4; + q.vy = (u.y + v.x)/s; + q.vz = (n.x + u.z)/s; + } else if(v.y > n.z) { + s = 2*sqrt(1 - u.x + v.y - n.z); + q.w = (n.x - u.z)/s; + q.vx = (u.y + v.x)/s; + q.vy = s/4; + q.vz = (v.z + n.y)/s; + } else { + s = 2*sqrt(1 - u.x - v.y + n.z); + q.w = (u.y - v.x)/s; + q.vx = (n.x + u.z)/s; + q.vy = (v.z + n.y)/s; + q.vz = s/4; + } + } + + return q.WithMagnitude(1); +} + +Quaternion Quaternion::Plus(Quaternion b) const { + Quaternion q; + q.w = w + b.w; + q.vx = vx + b.vx; + q.vy = vy + b.vy; + q.vz = vz + b.vz; + return q; +} + +Quaternion Quaternion::Minus(Quaternion b) const { + Quaternion q; + q.w = w - b.w; + q.vx = vx - b.vx; + q.vy = vy - b.vy; + q.vz = vz - b.vz; + return q; +} + +Quaternion Quaternion::ScaledBy(double s) const { + Quaternion q; + q.w = w*s; + q.vx = vx*s; + q.vy = vy*s; + q.vz = vz*s; + return q; +} + +double Quaternion::Magnitude() const { + return sqrt(w*w + vx*vx + vy*vy + vz*vz); +} + +Quaternion Quaternion::WithMagnitude(double s) const { + return ScaledBy(s/Magnitude()); +} + +Vector Quaternion::RotationU() const { + Vector v; + v.x = w*w + vx*vx - vy*vy - vz*vz; + v.y = 2*w *vz + 2*vx*vy; + v.z = 2*vx*vz - 2*w *vy; + return v; +} + +Vector Quaternion::RotationV() const { + Vector v; + v.x = 2*vx*vy - 2*w*vz; + v.y = w*w - vx*vx + vy*vy - vz*vz; + v.z = 2*w*vx + 2*vy*vz; + return v; +} + +Vector Quaternion::RotationN() const { + Vector v; + v.x = 2*w*vy + 2*vx*vz; + v.y = 2*vy*vz - 2*w*vx; + v.z = w*w - vx*vx - vy*vy + vz*vz; + return v; +} + +Vector Quaternion::Rotate(Vector p) const { + // Express the point in the new basis + return (RotationU().ScaledBy(p.x)).Plus( + RotationV().ScaledBy(p.y)).Plus( + RotationN().ScaledBy(p.z)); +} + +Quaternion Quaternion::Inverse() const { + Quaternion r; + r.w = w; + r.vx = -vx; + r.vy = -vy; + r.vz = -vz; + return r.WithMagnitude(1); // not that the normalize should be reqd +} + +Quaternion Quaternion::ToThe(double p) const { + // Avoid division by zero, or arccos of something not in its domain + if(w >= (1 - 1e-6)) { + return From(1, 0, 0, 0); + } else if(w <= (-1 + 1e-6)) { + return From(-1, 0, 0, 0); + } + + Quaternion r; + Vector axis = Vector::From(vx, vy, vz); + double theta = acos(w); // okay, since magnitude is 1, so -1 <= w <= 1 + theta *= p; + r.w = cos(theta); + axis = axis.WithMagnitude(sin(theta)); + r.vx = axis.x; + r.vy = axis.y; + r.vz = axis.z; + return r; +} + +Quaternion Quaternion::Times(Quaternion b) const { + double sa = w, sb = b.w; + Vector va = { vx, vy, vz }; + Vector vb = { b.vx, b.vy, b.vz }; + + Quaternion r; + r.w = sa*sb - va.Dot(vb); + Vector vr = vb.ScaledBy(sa).Plus( + va.ScaledBy(sb).Plus( + va.Cross(vb))); + r.vx = vr.x; + r.vy = vr.y; + r.vz = vr.z; + return r; +} + +Quaternion Quaternion::Mirror() const { + Vector u = RotationU(), + v = RotationV(); + u = u.ScaledBy(-1); + v = v.ScaledBy(-1); + return Quaternion::From(u, v); +} + + +Vector Vector::From(double x, double y, double z) { + Vector v; + v.x = x; v.y = y; v.z = z; + return v; +} + +Vector Vector::From(hParam x, hParam y, hParam z) { + Vector v; + v.x = SK.GetParam(x)->val; + v.y = SK.GetParam(y)->val; + v.z = SK.GetParam(z)->val; + return v; +} + +double Vector::Element(int i) const { + switch(i) { + case 0: return x; + case 1: return y; + case 2: return z; + default: ssassert(false, "Unexpected vector element index"); + } +} + +bool Vector::Equals(Vector v, double tol) const { + // Quick axis-aligned tests before going further + double dx = v.x - x; if(dx < -tol || dx > tol) return false; + double dy = v.y - y; if(dy < -tol || dy > tol) return false; + double dz = v.z - z; if(dz < -tol || dz > tol) return false; + + return (this->Minus(v)).MagSquared() < tol*tol; +} + +bool Vector::EqualsExactly(Vector v) const { + return EXACT(x == v.x && + y == v.y && + z == v.z); +} + +Vector Vector::Plus(Vector b) const { + Vector r; + + r.x = x + b.x; + r.y = y + b.y; + r.z = z + b.z; + + return r; +} + +Vector Vector::Minus(Vector b) const { + Vector r; + + r.x = x - b.x; + r.y = y - b.y; + r.z = z - b.z; + + return r; +} + +Vector Vector::Negated() const { + Vector r; + + r.x = -x; + r.y = -y; + r.z = -z; + + return r; +} + +Vector Vector::Cross(Vector b) const { + Vector r; + + r.x = -(z*b.y) + (y*b.z); + r.y = (z*b.x) - (x*b.z); + r.z = -(y*b.x) + (x*b.y); + + return r; +} + +double Vector::Dot(Vector b) const { + return (x*b.x + y*b.y + z*b.z); +} + +double Vector::DirectionCosineWith(Vector b) const { + Vector a = this->WithMagnitude(1); + b = b.WithMagnitude(1); + return a.Dot(b); +} + +Vector Vector::Normal(int which) const { + Vector n; + + // Arbitrarily choose one vector that's normal to us, pivoting + // appropriately. + double xa = fabs(x), ya = fabs(y), za = fabs(z); + if(this->Equals(Vector::From(0, 0, 1))) { + // Make DXFs exported in the XY plane work nicely... + n = Vector::From(1, 0, 0); + } else if(xa < ya && xa < za) { + n.x = 0; + n.y = z; + n.z = -y; + } else if(ya < za) { + n.x = -z; + n.y = 0; + n.z = x; + } else { + n.x = y; + n.y = -x; + n.z = 0; + } + + if(which == 0) { + // That's the vector we return. + } else if(which == 1) { + n = this->Cross(n); + } else ssassert(false, "Unexpected vector normal index"); + + n = n.WithMagnitude(1); + + return n; +} + +Vector Vector::RotatedAbout(Vector orig, Vector axis, double theta) const { + Vector r = this->Minus(orig); + r = r.RotatedAbout(axis, theta); + return r.Plus(orig); +} + +Vector Vector::RotatedAbout(Vector axis, double theta) const { + double c = cos(theta); + double s = sin(theta); + + axis = axis.WithMagnitude(1); + + Vector r; + + r.x = (x)*(c + (1 - c)*(axis.x)*(axis.x)) + + (y)*((1 - c)*(axis.x)*(axis.y) - s*(axis.z)) + + (z)*((1 - c)*(axis.x)*(axis.z) + s*(axis.y)); + + r.y = (x)*((1 - c)*(axis.y)*(axis.x) + s*(axis.z)) + + (y)*(c + (1 - c)*(axis.y)*(axis.y)) + + (z)*((1 - c)*(axis.y)*(axis.z) - s*(axis.x)); + + r.z = (x)*((1 - c)*(axis.z)*(axis.x) - s*(axis.y)) + + (y)*((1 - c)*(axis.z)*(axis.y) + s*(axis.x)) + + (z)*(c + (1 - c)*(axis.z)*(axis.z)); + + return r; +} + +Vector Vector::DotInToCsys(Vector u, Vector v, Vector n) const { + Vector r = { + this->Dot(u), + this->Dot(v), + this->Dot(n) + }; + return r; +} + +Vector Vector::ScaleOutOfCsys(Vector u, Vector v, Vector n) const { + Vector r = u.ScaledBy(x).Plus( + v.ScaledBy(y).Plus( + n.ScaledBy(z))); + return r; +} + +Vector Vector::InPerspective(Vector u, Vector v, Vector n, + Vector origin, double cameraTan) const +{ + Vector r = this->Minus(origin); + r = r.DotInToCsys(u, v, n); + // yes, minus; we are assuming a csys where u cross v equals n, backwards + // from the display stuff + double w = (1 - r.z*cameraTan); + r = r.ScaledBy(1/w); + + return r; +} + +double Vector::DistanceToLine(Vector p0, Vector dp) const { + double m = dp.Magnitude(); + return ((this->Minus(p0)).Cross(dp)).Magnitude() / m; +} + +double Vector::DistanceToPlane(Vector normal, Vector origin) const { + return this->Dot(normal) - origin.Dot(normal); +} + +bool Vector::OnLineSegment(Vector a, Vector b, double tol) const { + if(this->Equals(a, tol) || this->Equals(b, tol)) return true; + + Vector d = b.Minus(a); + + double m = d.MagSquared(); + double distsq = ((this->Minus(a)).Cross(d)).MagSquared() / m; + + if(distsq >= tol*tol) return false; + + double t = (this->Minus(a)).DivPivoting(d); + // On-endpoint already tested + if(t < 0 || t > 1) return false; + return true; +} + +Vector Vector::ClosestPointOnLine(Vector p0, Vector dp) const { + dp = dp.WithMagnitude(1); + // this, p0, and (p0+dp) define a plane; the min distance is in + // that plane, so calculate its normal + Vector pn = (this->Minus(p0)).Cross(dp); + // The minimum distance line is in that plane, perpendicular + // to the line + Vector n = pn.Cross(dp); + + // Calculate the actual distance + double d = (dp.Cross(p0.Minus(*this))).Magnitude(); + return this->Plus(n.WithMagnitude(d)); +} + +double Vector::MagSquared() const { + return x*x + y*y + z*z; +} + +double Vector::Magnitude() const { + return sqrt(x*x + y*y + z*z); +} + +Vector Vector::ScaledBy(double v) const { + Vector r; + + r.x = x * v; + r.y = y * v; + r.z = z * v; + + return r; +} + +Vector Vector::WithMagnitude(double v) const { + double m = Magnitude(); + if(EXACT(m == 0)) { + // We can do a zero vector with zero magnitude, but not any other cases. + if(fabs(v) > 1e-100) { + dbp("Vector::WithMagnitude(%g) of zero vector!", v); + } + return From(0, 0, 0); + } else { + return ScaledBy(v/m); + } +} + +Vector Vector::ProjectVectorInto(hEntity wrkpl) const { + EntityBase *w = SK.GetEntity(wrkpl); + Vector u = w->Normal()->NormalU(); + Vector v = w->Normal()->NormalV(); + + double up = this->Dot(u); + double vp = this->Dot(v); + + return (u.ScaledBy(up)).Plus(v.ScaledBy(vp)); +} + +Vector Vector::ProjectInto(hEntity wrkpl) const { + EntityBase *w = SK.GetEntity(wrkpl); + Vector p0 = w->WorkplaneGetOffset(); + + Vector f = this->Minus(p0); + + return p0.Plus(f.ProjectVectorInto(wrkpl)); +} + +Point2d Vector::Project2d(Vector u, Vector v) const { + Point2d p; + p.x = this->Dot(u); + p.y = this->Dot(v); + return p; +} + +Point2d Vector::ProjectXy() const { + Point2d p; + p.x = x; + p.y = y; + return p; +} + +Vector4 Vector::Project4d() const { + return Vector4::From(1, x, y, z); +} + +double Vector::DivPivoting(Vector delta) const { + double mx = fabs(delta.x), my = fabs(delta.y), mz = fabs(delta.z); + + if(mx > my && mx > mz) { + return x/delta.x; + } else if(my > mz) { + return y/delta.y; + } else { + return z/delta.z; + } +} + +Vector Vector::ClosestOrtho() const { + double mx = fabs(x), my = fabs(y), mz = fabs(z); + + if(mx > my && mx > mz) { + return From((x > 0) ? 1 : -1, 0, 0); + } else if(my > mz) { + return From(0, (y > 0) ? 1 : -1, 0); + } else { + return From(0, 0, (z > 0) ? 1 : -1); + } +} + +Vector Vector::ClampWithin(double minv, double maxv) const { + Vector ret = *this; + + if(ret.x < minv) ret.x = minv; + if(ret.y < minv) ret.y = minv; + if(ret.z < minv) ret.z = minv; + + if(ret.x > maxv) ret.x = maxv; + if(ret.y > maxv) ret.y = maxv; + if(ret.z > maxv) ret.z = maxv; + + return ret; +} + +void Vector::MakeMaxMin(Vector *maxv, Vector *minv) const { + maxv->x = max(maxv->x, x); + maxv->y = max(maxv->y, y); + maxv->z = max(maxv->z, z); + + minv->x = min(minv->x, x); + minv->y = min(minv->y, y); + minv->z = min(minv->z, z); +} + +bool Vector::OutsideAndNotOn(Vector maxv, Vector minv) const { + return (x > maxv.x + LENGTH_EPS) || (x < minv.x - LENGTH_EPS) || + (y > maxv.y + LENGTH_EPS) || (y < minv.y - LENGTH_EPS) || + (z > maxv.z + LENGTH_EPS) || (z < minv.z - LENGTH_EPS); +} + +bool Vector::BoundingBoxesDisjoint(Vector amax, Vector amin, + Vector bmax, Vector bmin) +{ + int i; + for(i = 0; i < 3; i++) { + if(amax.Element(i) < bmin.Element(i) - LENGTH_EPS) return true; + if(amin.Element(i) > bmax.Element(i) + LENGTH_EPS) return true; + } + return false; +} + +#if FULL_LIB_JJS +bool Vector::BoundingBoxIntersectsLine(Vector amax, Vector amin, + Vector p0, Vector p1, bool asSegment) +{ + Vector dp = p1.Minus(p0); + double lp = dp.Magnitude(); + dp = dp.ScaledBy(1.0/lp); + + int i, a; + for(i = 0; i < 3; i++) { + int j = WRAP(i+1, 3), k = WRAP(i+2, 3); + if(lp*fabs(dp.Element(i)) < LENGTH_EPS) continue; // parallel to plane + + for(a = 0; a < 2; a++) { + double d = (a == 0) ? amax.Element(i) : amin.Element(i); + // n dot (p0 + t*dp) = d + // (n dot p0) + t * (n dot dp) = d + double t = (d - p0.Element(i)) / dp.Element(i); + Vector p = p0.Plus(dp.ScaledBy(t)); + + if(asSegment && (t < -LENGTH_EPS || t > (lp+LENGTH_EPS))) continue; + + if(p.Element(j) > amax.Element(j) + LENGTH_EPS) continue; + if(p.Element(k) > amax.Element(k) + LENGTH_EPS) continue; + + if(p.Element(j) < amin.Element(j) - LENGTH_EPS) continue; + if(p.Element(k) < amin.Element(k) - LENGTH_EPS) continue; + + return true; + } + } + + return false; +} +#endif + +Vector Vector::AtIntersectionOfPlanes(Vector n1, double d1, + Vector n2, double d2) +{ + double det = (n1.Dot(n1))*(n2.Dot(n2)) - + (n1.Dot(n2))*(n1.Dot(n2)); + double c1 = (d1*n2.Dot(n2) - d2*n1.Dot(n2))/det; + double c2 = (d2*n1.Dot(n1) - d1*n1.Dot(n2))/det; + + return (n1.ScaledBy(c1)).Plus(n2.ScaledBy(c2)); +} + +void Vector::ClosestPointBetweenLines(Vector a0, Vector da, + Vector b0, Vector db, + double *ta, double *tb) +{ + // Make a semi-orthogonal coordinate system from those directions; + // note that dna and dnb need not be perpendicular. + Vector dn = da.Cross(db); // normal to both + Vector dna = dn.Cross(da); // normal to da + Vector dnb = dn.Cross(db); // normal to db + + // At the intersection of the lines + // a0 + pa*da = b0 + pb*db (where pa, pb are scalar params) + // So dot this equation against dna and dnb to get two equations + // to solve for da and db + *tb = ((a0.Minus(b0)).Dot(dna))/(db.Dot(dna)); + *ta = -((a0.Minus(b0)).Dot(dnb))/(da.Dot(dnb)); +} + +Vector Vector::AtIntersectionOfLines(Vector a0, Vector a1, + Vector b0, Vector b1, + bool *skew, + double *parama, double *paramb) +{ + Vector da = a1.Minus(a0), db = b1.Minus(b0); + + double pa, pb; + Vector::ClosestPointBetweenLines(a0, da, b0, db, &pa, &pb); + + if(parama) *parama = pa; + if(paramb) *paramb = pb; + + // And from either of those, we get the intersection point. + Vector pi = a0.Plus(da.ScaledBy(pa)); + + if(skew) { + // Check if the intersection points on each line are actually + // coincident... + if(pi.Equals(b0.Plus(db.ScaledBy(pb)))) { + *skew = false; + } else { + *skew = true; + } + } + return pi; +} + +Vector Vector::AtIntersectionOfPlaneAndLine(Vector n, double d, + Vector p0, Vector p1, + bool *parallel) +{ + Vector dp = p1.Minus(p0); + + if(fabs(n.Dot(dp)) < LENGTH_EPS) { + if(parallel) *parallel = true; + return Vector::From(0, 0, 0); + } + + if(parallel) *parallel = false; + + // n dot (p0 + t*dp) = d + // (n dot p0) + t * (n dot dp) = d + double t = (d - n.Dot(p0)) / (n.Dot(dp)); + + return p0.Plus(dp.ScaledBy(t)); +} + +static double det2(double a1, double b1, + double a2, double b2) +{ + return (a1*b2) - (b1*a2); +} +static double det3(double a1, double b1, double c1, + double a2, double b2, double c2, + double a3, double b3, double c3) +{ + return a1*det2(b2, c2, b3, c3) - + b1*det2(a2, c2, a3, c3) + + c1*det2(a2, b2, a3, b3); +} +Vector Vector::AtIntersectionOfPlanes(Vector na, double da, + Vector nb, double db, + Vector nc, double dc, + bool *parallel) +{ + double det = det3(na.x, na.y, na.z, + nb.x, nb.y, nb.z, + nc.x, nc.y, nc.z); + if(fabs(det) < 1e-10) { // arbitrary tolerance, not so good + *parallel = true; + return Vector::From(0, 0, 0); + } + *parallel = false; + + double detx = det3(da, na.y, na.z, + db, nb.y, nb.z, + dc, nc.y, nc.z); + + double dety = det3(na.x, da, na.z, + nb.x, db, nb.z, + nc.x, dc, nc.z); + + double detz = det3(na.x, na.y, da, + nb.x, nb.y, db, + nc.x, nc.y, dc ); + + return Vector::From(detx/det, dety/det, detz/det); +} + +size_t VectorHash::operator()(const Vector &v) const { + const size_t size = (size_t)pow(std::numeric_limits::max(), 1.0 / 3.0) - 1; + const double eps = 4.0 * LENGTH_EPS; + + double x = fabs(v.x) / eps; + double y = fabs(v.y) / eps; + double z = fabs(v.y) / eps; + + size_t xs = size_t(fmod(x, (double)size)); + size_t ys = size_t(fmod(y, (double)size)); + size_t zs = size_t(fmod(z, (double)size)); + + return (zs * size + ys) * size + xs; +} + +bool VectorPred::operator()(Vector a, Vector b) const { + return a.Equals(b, LENGTH_EPS); +} + +Vector4 Vector4::From(double w, double x, double y, double z) { + Vector4 ret; + ret.w = w; + ret.x = x; + ret.y = y; + ret.z = z; + return ret; +} + +Vector4 Vector4::From(double w, Vector v) { + return Vector4::From(w, w*v.x, w*v.y, w*v.z); +} + +Vector4 Vector4::Blend(Vector4 a, Vector4 b, double t) { + return (a.ScaledBy(1 - t)).Plus(b.ScaledBy(t)); +} + +Vector4 Vector4::Plus(Vector4 b) const { + return Vector4::From(w + b.w, x + b.x, y + b.y, z + b.z); +} + +Vector4 Vector4::Minus(Vector4 b) const { + return Vector4::From(w - b.w, x - b.x, y - b.y, z - b.z); +} + +Vector4 Vector4::ScaledBy(double s) const { + return Vector4::From(w*s, x*s, y*s, z*s); +} + +Vector Vector4::PerspectiveProject() const { + return Vector::From(x / w, y / w, z / w); +} + +Point2d Point2d::From(double x, double y) { + return { x, y }; +} + +Point2d Point2d::FromPolar(double r, double a) { + return { r * cos(a), r * sin(a) }; +} + +double Point2d::Angle() const { + double a = atan2(y, x); + return M_PI + remainder(a - M_PI, 2 * M_PI); +} + +double Point2d::AngleTo(const Point2d &p) const { + return p.Minus(*this).Angle(); +} + +Point2d Point2d::Plus(const Point2d &b) const { + return { x + b.x, y + b.y }; +} + +Point2d Point2d::Minus(const Point2d &b) const { + return { x - b.x, y - b.y }; +} + +Point2d Point2d::ScaledBy(double s) const { + return { x * s, y * s }; +} + +double Point2d::DivPivoting(Point2d delta) const { + if(fabs(delta.x) > fabs(delta.y)) { + return x/delta.x; + } else { + return y/delta.y; + } +} + +double Point2d::MagSquared() const { + return x*x + y*y; +} + +double Point2d::Magnitude() const { + return sqrt(x*x + y*y); +} + +Point2d Point2d::WithMagnitude(double v) const { + double m = Magnitude(); + if(m < 1e-20) { + dbp("!!! WithMagnitude() of zero vector"); + return { v, 0 }; + } + return { x * v / m, y * v / m }; +} + +double Point2d::DistanceTo(const Point2d &p) const { + double dx = x - p.x; + double dy = y - p.y; + return sqrt(dx*dx + dy*dy); +} + +double Point2d::Dot(Point2d p) const { + return x*p.x + y*p.y; +} + +double Point2d::DistanceToLine(const Point2d &p0, const Point2d &dp, bool asSegment) const { + double m = dp.x*dp.x + dp.y*dp.y; + if(m < LENGTH_EPS*LENGTH_EPS) return VERY_POSITIVE; + + // Let our line be p = p0 + t*dp, for a scalar t from 0 to 1 + double t = (dp.x*(x - p0.x) + dp.y*(y - p0.y))/m; + + if(asSegment) { + if(t < 0.0) return DistanceTo(p0); + if(t > 1.0) return DistanceTo(p0.Plus(dp)); + } + Point2d closest = p0.Plus(dp.ScaledBy(t)); + return DistanceTo(closest); +} + +double Point2d::DistanceToLineSigned(const Point2d &p0, const Point2d &dp, bool asSegment) const { + double m = dp.x*dp.x + dp.y*dp.y; + if(m < LENGTH_EPS*LENGTH_EPS) return VERY_POSITIVE; + + Point2d n = dp.Normal().WithMagnitude(1.0); + double dist = n.Dot(*this) - n.Dot(p0); + if(asSegment) { + // Let our line be p = p0 + t*dp, for a scalar t from 0 to 1 + double t = (dp.x*(x - p0.x) + dp.y*(y - p0.y))/m; + double sign = (dist > 0.0) ? 1.0 : -1.0; + if(t < 0.0) return DistanceTo(p0) * sign; + if(t > 1.0) return DistanceTo(p0.Plus(dp)) * sign; + } + + return dist; +} + +Point2d Point2d::Normal() const { + return { y, -x }; +} + +bool Point2d::Equals(Point2d v, double tol) const { + double dx = v.x - x; if(dx < -tol || dx > tol) return false; + double dy = v.y - y; if(dy < -tol || dy > tol) return false; + + return (this->Minus(v)).MagSquared() < tol*tol; +} + +BBox BBox::From(const Vector &p0, const Vector &p1) { + BBox bbox; + bbox.minp.x = min(p0.x, p1.x); + bbox.minp.y = min(p0.y, p1.y); + bbox.minp.z = min(p0.z, p1.z); + + bbox.maxp.x = max(p0.x, p1.x); + bbox.maxp.y = max(p0.y, p1.y); + bbox.maxp.z = max(p0.z, p1.z); + return bbox; +} + +Vector BBox::GetOrigin() const { return minp.Plus(maxp.Minus(minp).ScaledBy(0.5)); } +Vector BBox::GetExtents() const { return maxp.Minus(minp).ScaledBy(0.5); } + +void BBox::Include(const Vector &v, double r) { + minp.x = min(minp.x, v.x - r); + minp.y = min(minp.y, v.y - r); + minp.z = min(minp.z, v.z - r); + + maxp.x = max(maxp.x, v.x + r); + maxp.y = max(maxp.y, v.y + r); + maxp.z = max(maxp.z, v.z + r); +} + +bool BBox::Overlaps(const BBox &b1) const { + Vector t = b1.GetOrigin().Minus(GetOrigin()); + Vector e = b1.GetExtents().Plus(GetExtents()); + + return fabs(t.x) < e.x && fabs(t.y) < e.y && fabs(t.z) < e.z; +} + +bool BBox::Contains(const Point2d &p, double r) const { + return p.x >= (minp.x - r) && + p.y >= (minp.y - r) && + p.x <= (maxp.x + r) && + p.y <= (maxp.y + r); +} + +const std::vector& SolveSpace::StipplePatternDashes(StipplePattern pattern) { + static bool initialized; + static std::vector dashes[(size_t)StipplePattern::LAST + 1]; + if(!initialized) { + // Inkscape ignores all elements that are exactly zero instead of drawing + // them as dots, so set those to 1e-6. + dashes[(size_t)StipplePattern::CONTINUOUS] = + {}; + dashes[(size_t)StipplePattern::SHORT_DASH] = + { 1.0, 2.0 }; + dashes[(size_t)StipplePattern::DASH] = + { 1.0, 1.0 }; + dashes[(size_t)StipplePattern::DASH_DOT] = + { 1.0, 0.5, 1e-6, 0.5 }; + dashes[(size_t)StipplePattern::DASH_DOT_DOT] = + { 1.0, 0.5, 1e-6, 0.5, 1e-6, 0.5 }; + dashes[(size_t)StipplePattern::DOT] = + { 1e-6, 0.5 }; + dashes[(size_t)StipplePattern::LONG_DASH] = + { 2.0, 0.5 }; + dashes[(size_t)StipplePattern::FREEHAND] = + { 1.0, 2.0 }; + dashes[(size_t)StipplePattern::ZIGZAG] = + { 1.0, 2.0 }; + } + + return dashes[(size_t)pattern]; +} + +double SolveSpace::StipplePatternLength(StipplePattern pattern) { + static bool initialized; + static double lengths[(size_t)StipplePattern::LAST + 1]; + if(!initialized) { + for(size_t i = 0; i < (size_t)StipplePattern::LAST; i++) { + const std::vector &dashes = StipplePatternDashes((StipplePattern)i); + double length = 0.0; + for(double dash : dashes) { + length += dash; + } + lengths[i] = length; + } + } + + return lengths[(size_t)pattern]; +}