Implement Power function

OndselSolver/ASMTTime.cpp

@@ -22,6 +22,10 @@ std::shared_ptr<ASMTTime> MbD::ASMTTime::With()
 	return asmt;
 }
 
+MbD::ASMTTime::ASMTTime(Symsptr arg) : ExpressionX(arg)
+{
+}
+
 void MbD::ASMTTime::deleteMbD()
 {
 	xx = nullptr;
@@ -56,3 +60,8 @@ void MbD::ASMTTime::setValue(double val)
 {
 	xx->setValue(val);
 }
+
+Symsptr MbD::ASMTTime::copyWith(Symsptr arg)
+{
+	return std::make_shared<ASMTTime>(arg);
+}

OndselSolver/ASMTTime.h

@@ -18,10 +18,13 @@ namespace MbD {
         //
     public:
         static std::shared_ptr<ASMTTime> With();
+        ASMTTime() = default;
+        ASMTTime(Symsptr arg);
         void deleteMbD();
         void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
         Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
         Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        Symsptr copyWith(Symsptr arg) override;
         bool isVariable() override;
         void setValue(double val) override;
 

OndselSolver/Array.h

@@ -78,13 +78,17 @@ namespace MbD {
 			this->at(i) = x->at(i);
 		}
 	}
-	template<typename T>
-	inline void Array<T>::zeroSelf()
-	{
-		for (size_t i = 0; i < this->size(); i++) {
-			this->at(i) = (T)0;
-		}
-	}
+    template<typename T>
+    inline void Array<T>::zeroSelf()
+    {
+        for (size_t i = 0; i < this->size(); i++) {
+            if constexpr (std::is_pointer<T>::value) {
+                this->at(i) = nullptr; // Assign nullptr for pointer types
+            } else {
+                this->at(i) = T(); // Use default constructor for non-pointer types
+            }
+        }
+    }
 	template<typename T>
 	inline double Array<T>::rootMeanSquare()
 	{

OndselSolver/DifferentiatedGeneralSpline.cpp

@@ -37,3 +37,10 @@ std::ostream& MbD::DifferentiatedGeneralSpline::printOn(std::ostream& s) const
 	s << "deriv(" << *generalSpline << ", " << derivativeOrder << ")";
 	return s;
 }
+
+Symsptr MbD::DifferentiatedGeneralSpline::copyWith(Symsptr arg)
+{
+	auto clone = clonesptr();
+	std::static_pointer_cast<FunctionX>(clone)->setX(arg);
+	return clone;
+}

OndselSolver/DifferentiatedGeneralSpline.h

@@ -20,6 +20,7 @@ namespace MbD {
         double getValue() override;
         Symsptr differentiateWRTx() override;
         Symsptr clonesptr() override;
+        Symsptr copyWith(Symsptr arg) override;
 
         std::ostream& printOn(std::ostream& s) const override;
 

OndselSolver/ExpressionX.cpp

@@ -11,6 +11,10 @@
 
 using namespace MbD;
 
+MbD::ExpressionX::ExpressionX(Symsptr arg) : FunctionX(arg)
+{
+}
+
 void MbD::ExpressionX::xexpression(Symsptr arg, Symsptr func)
 {
 	//"

OndselSolver/ExpressionX.h

@@ -16,6 +16,8 @@ namespace MbD {
         //
     public:
 
+        ExpressionX() = default;
+        ExpressionX(Symsptr arg);
         void xexpression(Symsptr arg, Symsptr func);
         Symsptr differentiateWRTx() override;
         Symsptr differentiateWRT(Symsptr var) override;

OndselSolver/FunctionX.cpp

@@ -19,6 +19,11 @@ MbD::FunctionX::FunctionX(Symsptr arg) : xx(arg)
 {
 }
 
+void MbD::FunctionX::setX(Symsptr arg)
+{
+	xx = arg;
+}
+
 void MbD::FunctionX::arguments(Symsptr args)
 {
 	auto arguments = std::static_pointer_cast<Arguments>(args);
@@ -26,9 +31,10 @@ void MbD::FunctionX::arguments(Symsptr args)
 	xx = arguments->terms->front();
 }
 
-Symsptr MbD::FunctionX::copyWith(Symsptr self)
+Symsptr MbD::FunctionX::copyWith(Symsptr arg)
 {
-	return self;
+	assert(false);
+	return Symsptr();
 }
 
 Symsptr MbD::FunctionX::expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)

OndselSolver/FunctionX.h

@@ -20,8 +20,9 @@ namespace MbD {
 	public:
 		FunctionX() = default;
 		FunctionX(Symsptr arg);
+		void setX(Symsptr arg);
 		void arguments(Symsptr args) override;
-		virtual Symsptr copyWith(Symsptr arg);
+		virtual Symsptr copyWith(Symsptr arg) = 0;
 		Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
 		Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
 		Symsptr differentiateWRT(Symsptr var) override;

OndselSolver/FunctionXY.cpp

@@ -8,6 +8,7 @@
 
 #include "FunctionXY.h"
 #include "Sum.h"
+#include "Constant.h"
 
 using namespace MbD;
 
@@ -19,6 +20,11 @@ MbD::FunctionXY::FunctionXY(Symsptr base, Symsptr exp) : x(base), y(exp)
 {
 }
 
+Symsptr MbD::FunctionXY::copyWith(Symsptr argx, Symsptr argy)
+{
+	return Symsptr();
+}
+
 void MbD::FunctionXY::arguments(Symsptr args)
 {
 	//args is a Sum with "terms" containing the actual arguments
@@ -28,7 +34,44 @@ void MbD::FunctionXY::arguments(Symsptr args)
 	y = sum->terms->at(1);
 }
 
+Symsptr MbD::FunctionXY::expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); });
+	if (itr != set->end()) return sptr;
+	auto newx = x->expandUntil(x, set);
+	auto newy = y->expandUntil(y, set);
+	auto copy = copyWith(newx, newy);
+	return copy;
+}
+
+Symsptr MbD::FunctionXY::simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); });
+	if (itr != set->end()) return sptr;
+	auto newx = x->simplifyUntil(x, set);
+	auto newy = y->simplifyUntil(y, set);
+	auto copy = copyWith(newx, newy);
+	return copy;
+}
+
+void MbD::FunctionXY::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
+{
+	x->createMbD(mbdSys, mbdUnits);
+	y->createMbD(mbdSys, mbdUnits);
+}
+
 bool MbD::FunctionXY::isConstant()
 {
 	return x->isConstant() && y->isConstant();
 }
+
+Symsptr MbD::FunctionXY::differentiateWRT(Symsptr var)
+{
+	if (this == var.get()) return sptrConstant(1.0);
+	auto dfdx = differentiateWRTx();
+	auto dfdy = differentiateWRTy();
+	auto dxdvar = x->differentiateWRT(var);
+	auto dydvar = y->differentiateWRT(var);
+	return Symbolic::sum(Symbolic::times(dfdx, dxdvar), Symbolic::times(dfdy, dydvar));
+}
+

OndselSolver/FunctionXY.h

@@ -20,7 +20,12 @@ namespace MbD {
     public:
         FunctionXY();
         FunctionXY(Symsptr base, Symsptr exp);
+        virtual Symsptr copyWith(Symsptr argx, Symsptr argy) = 0;
+        Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+        Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
         void arguments(Symsptr args) override;
+        void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        Symsptr differentiateWRT(Symsptr var) override;
         virtual Symsptr differentiateWRTx() = 0;
         virtual Symsptr differentiateWRTy() = 0;
         bool isConstant() override;

OndselSolver/GeneralSpline.cpp

@@ -259,3 +259,10 @@ std::ostream& MbD::GeneralSpline::printOn(std::ostream& s) const
 	s << "})" << std::endl;
 	return s;
 }
+
+Symsptr MbD::GeneralSpline::copyWith(Symsptr arg)
+{
+	auto clone = clonesptr();
+	std::static_pointer_cast<FunctionX>(clone)->setX(arg);
+	return clone;
+}

OndselSolver/GeneralSpline.h

@@ -32,6 +32,7 @@ namespace MbD {
 		void calcIndexAndDelta();
 		void searchIndexFromto(size_t start, size_t end);
 		Symsptr clonesptr() override;
+		Symsptr copyWith(Symsptr arg) override;
 		double y(double xxx);
 
 		std::ostream& printOn(std::ostream& s) const override;

OndselSolver/Integral.cpp

@@ -2,6 +2,10 @@
 
 using namespace MbD;
 
+MbD::Integral::Integral(Symsptr arg) : ExpressionX(arg)
+{
+}
+
 MbD::Integral::Integral(Symsptr, Symsptr)
 {
 	assert(false);
@@ -48,3 +52,8 @@ std::ostream& MbD::Integral::printOn(std::ostream& s) const
 	s << *integrationConstant;
 	return s;
 }
+
+Symsptr MbD::Integral::copyWith(Symsptr arg)
+{
+	return std::make_shared<Integral>(arg);
+}

OndselSolver/Integral.h

@@ -16,10 +16,12 @@ namespace MbD {
 	{
 	public:
 		Integral() = default;
+		Integral(Symsptr arg);
 		Integral(Symsptr var, Symsptr integrand);
 		void arguments(Symsptr args) override;
 		Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
 		Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+		Symsptr copyWith(Symsptr arg) override;
 		void setIntegrationConstant(double integConstant) override;
 
 		std::ostream& printOn(std::ostream& s) const override;

OndselSolver/Item.h

@@ -22,7 +22,7 @@ namespace MbD {
 	class Constraint;
 	class StateData;
 
-	class Item
+	class Item : public std::enable_shared_from_this<Item>
 	{
 		//name
 	public:

OndselSolver/PiecewiseFunction.cpp

@@ -12,9 +12,8 @@
 
 using namespace MbD;
 
-MbD::PiecewiseFunction::PiecewiseFunction()
+MbD::PiecewiseFunction::PiecewiseFunction(Symsptr arg) : FunctionXcParameter(arg)
 {
-	noop();
 }
 
 MbD::PiecewiseFunction::PiecewiseFunction(Symsptr var, std::shared_ptr<std::vector<Symsptr>> funcs, std::shared_ptr<std::vector<Symsptr>> trans)
@@ -121,3 +120,8 @@ std::ostream& MbD::PiecewiseFunction::printOn(std::ostream& s) const
 	s << "})" << std::endl;
 	return s;
 }
+
+Symsptr MbD::PiecewiseFunction::copyWith(Symsptr arg)
+{
+	return std::make_shared<PiecewiseFunction>(arg, functions, transitions);
+}

OndselSolver/PiecewiseFunction.h

@@ -11,27 +11,29 @@
 #include "FunctionXcParameter.h"
 
 namespace MbD {
-    class Symbolic;
-    using Symsptr = std::shared_ptr<Symbolic>;
-    
-    class PiecewiseFunction : public FunctionXcParameter
-    {
-        //functions transitions 
-        //func0 tran0 func1 tran1 func2
-    public:
-        PiecewiseFunction();
-        PiecewiseFunction(Symsptr var, std::shared_ptr<std::vector<Symsptr>> funcs, std::shared_ptr<std::vector<Symsptr>> trans);
-        Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
-        Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
-        Symsptr differentiateWRTx() override;
-        Symsptr integrateWRT(Symsptr var) override;
-        double getValue() override;
-        void arguments(Symsptr args) override;
+	class Symbolic;
+	using Symsptr = std::shared_ptr<Symbolic>;
 
-        std::ostream& printOn(std::ostream& s) const override;
+	class PiecewiseFunction : public FunctionXcParameter
+	{
+		//functions transitions 
+		//func0 tran0 func1 tran1 func2
+	public:
+		PiecewiseFunction() = default;
+		PiecewiseFunction(Symsptr arg);
+		PiecewiseFunction(Symsptr var, std::shared_ptr<std::vector<Symsptr>> funcs, std::shared_ptr<std::vector<Symsptr>> trans);
+		Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+		Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
+		Symsptr differentiateWRTx() override;
+		Symsptr integrateWRT(Symsptr var) override;
+		Symsptr copyWith(Symsptr arg) override;
+		double getValue() override;
+		void arguments(Symsptr args) override;
 
-        std::shared_ptr<std::vector<Symsptr>> functions = std::make_shared<std::vector<Symsptr>>();
-        std::shared_ptr<std::vector<Symsptr>> transitions = std::make_shared<std::vector<Symsptr>>();
+		std::ostream& printOn(std::ostream& s) const override;
 
-    };
+		std::shared_ptr<std::vector<Symsptr>> functions = std::make_shared<std::vector<Symsptr>>();
+		std::shared_ptr<std::vector<Symsptr>> transitions = std::make_shared<std::vector<Symsptr>>();
+
+	};
 }

OndselSolver/Polynomial.cpp

@@ -14,6 +14,10 @@
 
 using namespace MbD;
 
+MbD::Polynomial::Polynomial(Symsptr arg) : FunctionXcParameter(arg)
+{
+}
+
 MbD::Polynomial::Polynomial(Symsptr var, std::shared_ptr<std::vector<double>> coefficients)
 {
 	assert(!coefficients->empty());
@@ -120,3 +124,10 @@ std::ostream& MbD::Polynomial::printOn(std::ostream& s) const
 	s << "})";
 	return s;
 }
+
+Symsptr MbD::Polynomial::copyWith(Symsptr arg)
+{
+	auto clone = clonesptr();
+	std::static_pointer_cast<FunctionX>(clone)->setX(arg);
+	return clone;
+}

OndselSolver/Polynomial.h

@@ -15,12 +15,15 @@ namespace MbD {
     {
         //pn = a0*x^0 + a1*x^1 ... an*x^n
     public:
+        Polynomial() = default;
+        Polynomial(Symsptr arg);
         Polynomial(Symsptr var, std::shared_ptr<std::vector<double>> coeffs);
         Polynomial(Symsptr var, std::shared_ptr<std::vector<Symsptr>> coeffs);
         Symsptr expandUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
         Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
         Symsptr differentiateWRTx() override;
         Symsptr integrateWRT(Symsptr var) override;
+        Symsptr copyWith(Symsptr arg) override;
         double getValue() override;
         void setIntegrationConstant(double integConstant) override;
 

OndselSolver/Power.cpp

@@ -5,7 +5,7 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #include "Power.h"
 #include "Constant.h"
 #include "Ln.h"
@@ -35,13 +35,36 @@ Symsptr MbD::Power::differentiateWRTy()
 	return deriv->simplified();
 }
 
-Symsptr MbD::Power::simplifyUntil(Symsptr, std::shared_ptr<std::unordered_set<Symsptr>>)
+Symsptr MbD::Power::copyWith(Symsptr argx, Symsptr argy)
 {
-	assert(false);
-	return Symsptr();
+	return std::make_shared<Power>(argx, argy);
 }
 
 double MbD::Power::getValue()
 {
 	return std::pow(x->getValue(), y->getValue());
 }
+
+Symsptr MbD::Power::clonesptr()
+{
+	return std::make_shared<Power>(*this);
+}
+
+Symsptr MbD::Power::simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set)
+{
+	auto itr = std::find_if(set->begin(), set->end(), [sptr](Symsptr sym) {return sptr.get() == sym.get(); });
+	if (itr != set->end()) return sptr;
+	auto newx = x->simplifyUntil(x, set);
+	auto newy = y->simplifyUntil(y, set);
+	if (y->isConstant() && y->getValue() == 1) {
+		return newx;
+	}
+	auto copy = copyWith(newx, newy);
+	return copy;
+}
+
+std::ostream& MbD::Power::printOn(std::ostream& s) const
+{
+	s << "pow(" << *x << "," << *y << ")";
+	return s;
+}

OndselSolver/Power.h

@@ -19,10 +19,13 @@ namespace MbD {
         Power(Symsptr base, Symsptr exp);
         Symsptr differentiateWRTx() override;
         Symsptr differentiateWRTy() override;
-
+        Symsptr copyWith(Symsptr argx, Symsptr argy) override;
+        Symsptr clonesptr() override;
         Symsptr simplifyUntil(Symsptr sptr, std::shared_ptr<std::unordered_set<Symsptr>> set) override;
         double getValue() override;
 
+        std::ostream& printOn(std::ostream& s) const override;
+
     };
 }
 

OndselSolver/Product.cpp

@@ -5,7 +5,7 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #include <algorithm>
 #include <iterator>
 
@@ -24,7 +24,7 @@ Symsptr MbD::Product::differentiateWRT(Symsptr var)
 	std::transform(terms->begin(),
 		terms->end(),
 		std::back_inserter(*derivatives),
-		[var](Symsptr term) { 
+		[var](Symsptr term) {
 			return term->differentiateWRT(var);
 		}
 	);
@@ -162,7 +162,22 @@ bool Product::isProduct()
 double Product::getValue()
 {
 	double answer = 1.0;
-	for (size_t i = 0; i < terms->size(); i++) answer *= terms->at(i)->getValue();
+	bool hasInfinity = false;
+	for (size_t i = 0; i < terms->size(); i++)
+	{
+		double termValue = terms->at(i)->getValue();
+		if (termValue == 0.0) {
+			return 0.0; // If any term is zero, the product is zero
+		}
+		if (std::isfinite(termValue)) {
+			answer *= termValue;
+		}
+		else {
+			hasInfinity = true;
+			//Continue to look for a zero term.
+		}
+	}
+	assert(!hasInfinity);
 	return answer;
 }
 

OndselSolver/RampStepFunction.cpp

@@ -6,9 +6,12 @@
 
 using namespace MbD;
 
+MbD::RampStepFunction::RampStepFunction(Symsptr arg) : PiecewiseFunction(arg)
+{
+}
+
 MbD::RampStepFunction::RampStepFunction(Symsptr var, std::shared_ptr<std::vector<double>> consts, std::shared_ptr<std::vector<double>> trans)
 {
-
 	double x0 = trans->at(0);
 	double x1 = trans->at(1);
 	double y0 = consts->at(0);
@@ -60,3 +63,8 @@ void MbD::RampStepFunction::initFunctionsTransitions(Symsptr var, double x0, dou
 	transitions->push_back(symx0);
 	transitions->push_back(symx1);
 }
+
+Symsptr MbD::RampStepFunction::copyWith(Symsptr arg)
+{
+	return std::make_shared<RampStepFunction>(arg);
+}

OndselSolver/RampStepFunction.h

@@ -15,10 +15,12 @@ namespace MbD {
     {
     public:
         RampStepFunction() = default;
+        RampStepFunction(Symsptr arg);
         RampStepFunction(Symsptr var, std::shared_ptr<std::vector<double>> consts, std::shared_ptr<std::vector<double>> trans);
         void arguments(Symsptr args) override;
         void initFunctionsTransitions(Symsptr var, double x0, double y0, double x1, double y1);
         void initFunctionsTransitions(Symsptr var, double x0, double y0, double x1, double y1, Symsptr symx0, Symsptr symy0, Symsptr symx1, Symsptr symy1);
+        Symsptr copyWith(Symsptr arg) override;
 
     };
 }

OndselSolver/Sum.cpp

@@ -126,6 +126,7 @@ double Sum::getValue()
 {
 	double answer = 0.0;
 	for (size_t i = 0; i < terms->size(); i++) answer += terms->at(i)->getValue();
+	assert(std::isfinite(answer));
 	return answer;
 }
 

OndselSolverMain/OndselSolver.cpp

@@ -23,8 +23,8 @@ void sharedptrTest();
 
 int main()
 {
-	//ASMTAssembly::runFile("C:/Users/askoh/OneDrive/askoh/visualstudio/Ondsel/OndselFreeCAD/build/src/Main/runDragStep.asmt");
-	//return 0;
+	ASMTAssembly::runFile("C:/Users/askoh/Downloads/pistonDebug.asmt");
+	return 0;
 	//auto assembly = ASMTAssembly::assemblyFromFile("C:/Users/askoh/OneDrive/askoh/visualstudio/Ondsel/OndselFreeCAD/build/src/Main/runPreDrag.asmt");
 	//assembly->runDraggingLog("C:/Users/askoh/OneDrive/askoh/visualstudio/Ondsel/OndselFreeCAD/build/src/Main/dragging.log");
 	//return 0;