kindred/solver
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Merge pull request #4 from Ondsel-Development/working

Browse Source 
Working
This commit is contained in:
aiksiongkoh
2023-08-11 15:23:38 -06:00
committed by GitHub
parent 80bb812075 11cbcffacf
commit d5d7dcbc63
578 changed files with 22138 additions and 2206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

481
LICENSE Normal file
View File

@@ -0,0 +1,481 @@
GNU LIBRARY GENERAL PUBLIC LICENSE
Version 2, June 1991
Copyright (C) 1991 Free Software Foundation, Inc.
51 Franklin Street - Fifth Floor
Boston, MA 02110-1301, USA.
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
[This is the first released version of the library GPL. It is
numbered 2 because it goes with version 2 of the ordinary GPL.]
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
Licenses are intended to guarantee your freedom to share and change
free software--to make sure the software is free for all its users.
This license, the Library General Public License, applies to some
specially designated Free Software Foundation software, and to any
other libraries whose authors decide to use it. You can use it for
your libraries, 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
this service 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 make restrictions that forbid
anyone to deny you these rights or to ask you to surrender the rights.
These restrictions translate to certain responsibilities for you if
you distribute copies of the library, or if you modify it.
For example, if you distribute copies of the library, whether gratis
or for a fee, you must give the recipients all the rights that we gave
you. You must make sure that they, too, receive or can get the source
code. If you link a program with the library, you must provide
complete object files to the recipients so that they can relink them
with the library, after making changes to the library and recompiling
it. And you must show them these terms so they know their rights.
Our method of protecting your rights has two steps: (1) copyright
the library, and (2) offer you this license which gives you legal
permission to copy, distribute and/or modify the library.
Also, for each distributor's protection, we want to make certain
that everyone understands that there is no warranty for this free
library. If the library is modified by someone else and passed on, we
want its recipients to know that what they have is not the original
version, so that any problems introduced by others will not reflect on
the original authors' reputations.
Finally, any free program is threatened constantly by software
patents. We wish to avoid the danger that companies distributing free
software will individually obtain patent licenses, thus in effect
transforming the program into proprietary software. To prevent this,
we have made it clear that any patent must be licensed for everyone's
free use or not licensed at all.
Most GNU software, including some libraries, is covered by the ordinary
GNU General Public License, which was designed for utility programs. This
license, the GNU Library General Public License, applies to certain
designated libraries. This license is quite different from the ordinary
one; be sure to read it in full, and don't assume that anything in it is
the same as in the ordinary license.
The reason we have a separate public license for some libraries is that
they blur the distinction we usually make between modifying or adding to a
program and simply using it. Linking a program with a library, without
changing the library, is in some sense simply using the library, and is
analogous to running a utility program or application program. However, in
a textual and legal sense, the linked executable is a combined work, a
derivative of the original library, and the ordinary General Public License
treats it as such.
Because of this blurred distinction, using the ordinary General
Public License for libraries did not effectively promote software
sharing, because most developers did not use the libraries. We
concluded that weaker conditions might promote sharing better.
However, unrestricted linking of non-free programs would deprive the
users of those programs of all benefit from the free status of the
libraries themselves. This Library General Public License is intended to
permit developers of non-free programs to use free libraries, while
preserving your freedom as a user of such programs to change the free
libraries that are incorporated in them. (We have not seen how to achieve
this as regards changes in header files, but we have achieved it as regards
changes in the actual functions of the Library.) The hope is that this
will lead to faster development of free libraries.
The precise terms and conditions for copying, distribution and
modification follow. Pay close attention to the difference between a
"work based on the library" and a "work that uses the library". The
former contains code derived from the library, while the latter only
works together with the library.
Note that it is possible for a library to be covered by the ordinary
General Public License rather than by this special one.
GNU LIBRARY GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License Agreement applies to any software library which
contains a notice placed by the copyright holder or other authorized
party saying it may be distributed under the terms of this Library
General Public License (also called "this License"). Each licensee is
addressed as "you".
A "library" means a collection of software functions and/or data
prepared so as to be conveniently linked with application programs
(which use some of those functions and data) to form executables.
The "Library", below, refers to any such software library or work
which has been distributed under these terms. A "work based on the
Library" means either the Library or any derivative work under
copyright law: that is to say, a work containing the Library or a
portion of it, either verbatim or with modifications and/or translated
straightforwardly into another language. (Hereinafter, translation is
included without limitation in the term "modification".)
"Source code" for a work means the preferred form of the work for
making modifications to it. For a library, complete source code means
all the source code for all modules it contains, plus any associated
interface definition files, plus the scripts used to control compilation
and installation of the library.
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running a program using the Library is not restricted, and output from
such a program is covered only if its contents constitute a work based
on the Library (independent of the use of the Library in a tool for
writing it). Whether that is true depends on what the Library does
and what the program that uses the Library does.
1. You may copy and distribute verbatim copies of the Library's
complete source code as you receive it, in any medium, provided that
you conspicuously and appropriately publish on each copy an
appropriate copyright notice and disclaimer of warranty; keep intact
all the notices that refer to this License and to the absence of any
warranty; and distribute a copy of this License along with the
Library.
You may charge a fee for the physical act of transferring a copy,
and you may at your option offer warranty protection in exchange for a
fee.
2. You may modify your copy or copies of the Library or any portion
of it, thus forming a work based on the Library, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) The modified work must itself be a software library.
b) You must cause the files modified to carry prominent notices
stating that you changed the files and the date of any change.
c) You must cause the whole of the work to be licensed at no
charge to all third parties under the terms of this License.
d) If a facility in the modified Library refers to a function or a
table of data to be supplied by an application program that uses
the facility, other than as an argument passed when the facility
is invoked, then you must make a good faith effort to ensure that,
in the event an application does not supply such function or
table, the facility still operates, and performs whatever part of
its purpose remains meaningful.
(For example, a function in a library to compute square roots has
a purpose that is entirely well-defined independent of the
application. Therefore, Subsection 2d requires that any
application-supplied function or table used by this function must
be optional: if the application does not supply it, the square
root function must still compute square roots.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Library,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Library, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote
it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Library.
In addition, mere aggregation of another work not based on the Library
with the Library (or with a work based on the Library) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may opt to apply the terms of the ordinary GNU General Public
License instead of this License to a given copy of the Library. To do
this, you must alter all the notices that refer to this License, so
that they refer to the ordinary GNU General Public License, version 2,
instead of to this License. (If a newer version than version 2 of the
ordinary GNU General Public License has appeared, then you can specify
that version instead if you wish.) Do not make any other change in
these notices.
Once this change is made in a given copy, it is irreversible for
that copy, so the ordinary GNU General Public License applies to all
subsequent copies and derivative works made from that copy.
This option is useful when you wish to copy part of the code of
the Library into a program that is not a library.
4. You may copy and distribute the Library (or a portion or
derivative of it, under Section 2) in object code or executable form
under the terms of Sections 1 and 2 above provided that you accompany
it with the complete corresponding machine-readable source code, which
must be distributed under the terms of Sections 1 and 2 above on a
medium customarily used for software interchange.
If distribution of object code is made by offering access to copy
from a designated place, then offering equivalent access to copy the
source code from the same place satisfies the requirement to
distribute the source code, even though third parties are not
compelled to copy the source along with the object code.
5. A program that contains no derivative of any portion of the
Library, but is designed to work with the Library by being compiled or
linked with it, is called a "work that uses the Library". Such a
work, in isolation, is not a derivative work of the Library, and
therefore falls outside the scope of this License.
However, linking a "work that uses the Library" with the Library
creates an executable that is a derivative of the Library (because it
contains portions of the Library), rather than a "work that uses the
library". The executable is therefore covered by this License.
Section 6 states terms for distribution of such executables.
When a "work that uses the Library" uses material from a header file
that is part of the Library, the object code for the work may be a
derivative work of the Library even though the source code is not.
Whether this is true is especially significant if the work can be
linked without the Library, or if the work is itself a library. The
threshold for this to be true is not precisely defined by law.
If such an object file uses only numerical parameters, data
structure layouts and accessors, and small macros and small inline
functions (ten lines or less in length), then the use of the object
file is unrestricted, regardless of whether it is legally a derivative
work. (Executables containing this object code plus portions of the
Library will still fall under Section 6.)
Otherwise, if the work is a derivative of the Library, you may
distribute the object code for the work under the terms of Section 6.
Any executables containing that work also fall under Section 6,
whether or not they are linked directly with the Library itself.
6. As an exception to the Sections above, you may also compile or
link a "work that uses the Library" with the Library to produce a
work containing portions of the Library, and distribute that work
under terms of your choice, provided that the terms permit
modification of the work for the customer's own use and reverse
engineering for debugging such modifications.
You must give prominent notice with each copy of the work that the
Library is used in it and that the Library and its use are covered by
this License. You must supply a copy of this License. If the work
during execution displays copyright notices, you must include the
copyright notice for the Library among them, as well as a reference
directing the user to the copy of this License. Also, you must do one
of these things:
a) Accompany the work with the complete corresponding
machine-readable source code for the Library including whatever
changes were used in the work (which must be distributed under
Sections 1 and 2 above); and, if the work is an executable linked
with the Library, with the complete machine-readable "work that
uses the Library", as object code and/or source code, so that the
user can modify the Library and then relink to produce a modified
executable containing the modified Library. (It is understood
that the user who changes the contents of definitions files in the
Library will not necessarily be able to recompile the application
to use the modified definitions.)
b) Accompany the work with a written offer, valid for at
least three years, to give the same user the materials
specified in Subsection 6a, above, for a charge no more
than the cost of performing this distribution.
c) If distribution of the work is made by offering access to copy
from a designated place, offer equivalent access to copy the above
specified materials from the same place.
d) Verify that the user has already received a copy of these
materials or that you have already sent this user a copy.
For an executable, the required form of the "work that uses the
Library" must include any data and utility programs needed for
reproducing the executable from it. However, as a special exception,
the source code distributed need not include anything that is normally
distributed (in either source or binary form) with the major
components (compiler, kernel, and so on) of the operating system on
which the executable runs, unless that component itself accompanies
the executable.
It may happen that this requirement contradicts the license
restrictions of other proprietary libraries that do not normally
accompany the operating system. Such a contradiction means you cannot
use both them and the Library together in an executable that you
distribute.
7. You may place library facilities that are a work based on the
Library side-by-side in a single library together with other library
facilities not covered by this License, and distribute such a combined
library, provided that the separate distribution of the work based on
the Library and of the other library facilities is otherwise
permitted, and provided that you do these two things:
a) Accompany the combined library with a copy of the same work
based on the Library, uncombined with any other library
facilities. This must be distributed under the terms of the
Sections above.
b) Give prominent notice with the combined library of the fact
that part of it is a work based on the Library, and explaining
where to find the accompanying uncombined form of the same work.
8. You may not copy, modify, sublicense, link with, or distribute
the Library except as expressly provided under this License. Any
attempt otherwise to copy, modify, sublicense, link with, or
distribute the Library is void, and will automatically terminate your
rights under this License. However, parties who have received copies,
or rights, from you under this License will not have their licenses
terminated so long as such parties remain in full compliance.
9. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Library or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Library (or any work based on the
Library), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Library or works based on it.
10. Each time you redistribute the Library (or any work based on the
Library), the recipient automatically receives a license from the
original licensor to copy, distribute, link with or modify the Library
subject to these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties to
this License.
11. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
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
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Library at all. For example, if a patent
license would not permit royalty-free redistribution of the Library by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Library.
If any portion of this section is held invalid or unenforceable under any
particular circumstance, the balance of the section is intended to apply,
and the section as a whole is intended to apply in other circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
12. If the distribution and/or use of the Library is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Library under this License may add
an explicit geographical distribution limitation excluding those countries,
so that distribution is permitted only in or among countries not thus
excluded. In such case, this License incorporates the limitation as if
written in the body of this License.
13. The Free Software Foundation may publish revised and/or new
versions of the Library 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 Library
specifies a version number of this License which applies to it and
"any later version", you have the option of following the terms and
conditions either of that version or of any later version published by
the Free Software Foundation. If the Library does not specify a
license version number, you may choose any version ever published by
the Free Software Foundation.
14. If you wish to incorporate parts of the Library into other free
programs whose distribution conditions are incompatible with these,
write to the author to ask for permission. For software which is
copyrighted by the Free Software Foundation, write to the Free
Software Foundation; we sometimes make exceptions for this. Our
decision will be guided by the two goals of preserving the free status
of all derivatives of our free software and of promoting the sharing
and reuse of software generally.
NO WARRANTY
15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
OTHER PARTIES PROVIDE THE LIBRARY "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
LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
AND/OR REDISTRIBUTE THE LIBRARY 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
LIBRARY (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 LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
DAMAGES.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Libraries
If you develop a new library, and you want it to be of the greatest
possible use to the public, we recommend making it free software that
everyone can redistribute and change. You can do so by permitting
redistribution under these terms (or, alternatively, under the terms of the
ordinary General Public License).
To apply these terms, attach the following notices to the library. It is
safest to attach them to the start of each source file to most effectively
convey the exclusion of warranty; and each file should have at least the
"copyright" line and a pointer to where the full notice is found.
<one line to give the library's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
Also add information on how to contact you by electronic and paper mail.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the library, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the
library `Frob' (a library for tweaking knobs) written by James Random Hacker.
<signature of Ty Coon>, 1 April 1990
Ty Coon, President of Vice
That's all there is to it!

1494
MbDCode/00backhoe.asmt Normal file
View File

File diff suppressed because it is too large Load Diff

8
MbDCode/ASMTAnimationParameters.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTAnimationParameters.h"
using namespace MbD;

13
MbDCode/ASMTAnimationParameters.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
@@ -9,8 +17,9 @@ namespace MbD {
public:
void parseASMT(std::vector<std::string>& lines) override;
int nframe, icurrent, istart, iend, framesPerSecond;
bool isForward;
int nframe = 1000000, icurrent = 0, istart = 0, iend = 1000000, framesPerSecond = 30;
bool isForward = true;
};
}

873
MbDCode/ASMTAssembly.cpp
View File

@@ -1,6 +1,16 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include <string>
#include <cassert>
#include <fstream>
#include <algorithm>
#include <numeric>
#include "ASMTAssembly.h"
#include "CREATE.h"
@@ -8,16 +18,24 @@
#include "ASMTCylindricalJoint.h"
#include "ASMTRotationalMotion.h"
#include "ASMTTranslationalMotion.h"
#include "ASMTMarker.h"
#include "Part.h"
#include "ASMTTranslationalJoint.h"
#include "ASMTSphericalJoint.h"
#include "ASMTFixedJoint.h"
#include "ASMTGeneralMotion.h"
#include "ASMTUniversalJoint.h"
#include "ExternalSystem.h"
#include "ASMTPointInPlaneJoint.h"
using namespace MbD;
void MbD::ASMTAssembly::runFile(const char* chars)
{
std::string str;
std::ifstream in(chars);
std::ifstream stream(chars);
std::string line;
std::vector<std::string> lines;
while (std::getline(in, line)) {
while (std::getline(stream, line)) {
lines.push_back(line);
}
assert(lines[0] == "freeCAD: 3D CAD with Motion Simulation by askoh.com");
@@ -27,265 +45,638 @@ void MbD::ASMTAssembly::runFile(const char* chars)
lines.erase(lines.begin());
auto assembly = CREATE<ASMTAssembly>::With();
assembly->parseASMT(lines);
assembly->runKINEMATIC();
}
}
ASMTAssembly* MbD::ASMTAssembly::root()
{
return this;
}
void MbD::ASMTAssembly::parseASMT(std::vector<std::string>& lines)
{
size_t pos = lines[0].find_first_not_of("\t");
auto leadingTabs = lines[0].substr(0, pos);
while (!lines.empty()) {
if (lines[0] == (leadingTabs + "Notes")) {
lines.erase(lines.begin());
notes = lines[0];
lines.erase(lines.begin());
readNotes(lines);
readName(lines);
readPosition3D(lines);
readRotationMatrix(lines);
readVelocity3D(lines);
readOmega3D(lines);
initprincipalMassMarker();
readRefPoints(lines);
readRefCurves(lines);
readRefSurfaces(lines);
readParts(lines);
readKinematicIJs(lines);
readConstraintSets(lines);
readForcesTorques(lines);
readConstantGravity(lines);
readSimulationParameters(lines);
readAnimationParameters(lines);
readTimeSeries(lines);
readAssemblySeries(lines);
readPartSeriesMany(lines);
readJointSeriesMany(lines);
readMotionSeriesMany(lines);
}
void MbD::ASMTAssembly::readNotes(std::vector<std::string>& lines)
{
assert(lines[0] == "\tNotes");
lines.erase(lines.begin());
notes = readString(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTAssembly::readParts(std::vector<std::string>& lines)
{
assert(lines[0] == "\tParts");
lines.erase(lines.begin());
parts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
auto it = std::find(lines.begin(), lines.end(), "\tKinematicIJs");
std::vector<std::string> partsLines(lines.begin(), it);
while (!partsLines.empty()) {
readPart(partsLines);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTAssembly::readPart(std::vector<std::string>& lines)
{
assert(lines[0] == "\t\tPart");
lines.erase(lines.begin());
auto part = CREATE<ASMTPart>::With();
part->parseASMT(lines);
parts->push_back(part);
part->owner = this;
}
void MbD::ASMTAssembly::readKinematicIJs(std::vector<std::string>& lines)
{
assert(lines[0] == "\tKinematicIJs");
lines.erase(lines.begin());
kinematicIJs = std::make_shared<std::vector<std::shared_ptr<ASMTKinematicIJ>>>();
auto it = std::find(lines.begin(), lines.end(), "\tConstraintSets");
std::vector<std::string> kinematicIJsLines(lines.begin(), it);
while (!kinematicIJsLines.empty()) {
readKinematicIJ(kinematicIJsLines);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTAssembly::readKinematicIJ(std::vector<std::string>& lines)
{
assert(false);
}
void MbD::ASMTAssembly::readConstraintSets(std::vector<std::string>& lines)
{
assert(lines[0] == "\tConstraintSets");
lines.erase(lines.begin());
readJoints(lines);
readMotions(lines);
readGeneralConstraintSets(lines);
}
void MbD::ASMTAssembly::readJoints(std::vector<std::string>& lines)
{
assert(lines[0] == "\t\tJoints");
lines.erase(lines.begin());
joints = std::make_shared<std::vector<std::shared_ptr<ASMTJoint>>>();
auto it = std::find(lines.begin(), lines.end(), "\t\tMotions");
std::vector<std::string> jointsLines(lines.begin(), it);
std::shared_ptr<ASMTJoint> joint;
while (!jointsLines.empty()) {
if (jointsLines[0] == "\t\t\tRevoluteJoint") {
joint = CREATE<ASMTRevoluteJoint>::With();
}
else if (lines[0] == (leadingTabs + "Name")) {
lines.erase(lines.begin());
name = lines[0];
lines.erase(lines.begin());
else if (jointsLines[0] == "\t\t\tCylindricalJoint") {
joint = CREATE<ASMTCylindricalJoint>::With();
}
else if (lines[0] == (leadingTabs + "Position3D")) {
lines.erase(lines.begin());
std::istringstream iss(lines[0]);
position3D = std::make_shared<FullColumn<double>>();
double d;
while (iss >> d) {
position3D->push_back(d);
}
lines.erase(lines.begin());
else if (jointsLines[0] == "\t\t\tTranslationalJoint") {
joint = CREATE<ASMTTranslationalJoint>::With();
}
else if (lines[0] == (leadingTabs + "RotationMatrix")) {
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
for (int i = 0; i < 3; i++)
{
auto& row = rotationMatrix->at(i);
std::istringstream iss(lines[0]);
double d;
while (iss >> d) {
row->push_back(d);
}
lines.erase(lines.begin());
}
else if (jointsLines[0] == "\t\t\tSphericalJoint") {
joint = CREATE<ASMTSphericalJoint>::With();
}
else if (lines[0] == (leadingTabs + "Velocity3D")) {
lines.erase(lines.begin());
std::istringstream iss(lines[0]);
velocity3D = std::make_shared<FullColumn<double>>();
double d;
while (iss >> d) {
velocity3D->push_back(d);
}
lines.erase(lines.begin());
else if (jointsLines[0] == "\t\t\tFixedJoint") {
joint = CREATE<ASMTFixedJoint>::With();
}
else if (lines[0] == (leadingTabs + "Omega3D")) {
lines.erase(lines.begin());
std::istringstream iss(lines[0]);
omega3D = std::make_shared<FullColumn<double>>();
double d;
while (iss >> d) {
omega3D->push_back(d);
}
lines.erase(lines.begin());
else if (jointsLines[0] == "\t\t\tUniversalJoint") {
joint = CREATE<ASMTUniversalJoint>::With();
}
else if (lines[0] == (leadingTabs + "RefPoints")) {
lines.erase(lines.begin());
refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefCurves"));
std::vector<std::string> refPointsLines(lines.begin(), it);
while (!refPointsLines.empty()) {
if (refPointsLines[0] == (leadingTabs + "\tRefPoint")) {
refPointsLines.erase(refPointsLines.begin());
auto refPoint = CREATE<ASMTRefPoint>::With();
refPoint->parseASMT(refPointsLines);
refPoints->push_back(refPoint);
refPoint->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
else if (jointsLines[0] == "\t\t\tPointInPlaneJoint") {
joint = CREATE<ASMTPointInPlaneJoint>::With();
}
else if (lines[0] == (leadingTabs + "RefCurves")) {
lines.erase(lines.begin());
refCurves = std::make_shared<std::vector<std::shared_ptr<ASMTRefCurve>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefSurfaces"));
std::vector<std::string> refCurvesLines(lines.begin(), it);
while (!refCurvesLines.empty()) {
if (refCurvesLines[0] == (leadingTabs + "\tRefCurve")) {
refCurvesLines.erase(refCurvesLines.begin());
auto refCurve = CREATE<ASMTRefCurve>::With();
refCurve->parseASMT(refCurvesLines);
refCurves->push_back(refCurve);
refCurve->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
else {
assert(false);
}
else if (lines[0] == (leadingTabs + "RefSurfaces")) {
lines.erase(lines.begin());
refSurfaces = std::make_shared<std::vector<std::shared_ptr<ASMTRefSurface>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "Parts"));
std::vector<std::string> refSurfacesLines(lines.begin(), it);
while (!refSurfacesLines.empty()) {
if (refSurfacesLines[0] == (leadingTabs + "\tRefSurface")) {
refSurfacesLines.erase(refSurfacesLines.begin());
auto refSurface = CREATE<ASMTRefSurface>::With();
refSurface->parseASMT(refSurfacesLines);
refSurfaces->push_back(refSurface);
refSurface->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
jointsLines.erase(jointsLines.begin());
joint->parseASMT(jointsLines);
joints->push_back(joint);
joint->owner = this;
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTAssembly::readMotions(std::vector<std::string>& lines)
{
assert(lines[0] == "\t\tMotions");
lines.erase(lines.begin());
motions = std::make_shared<std::vector<std::shared_ptr<ASMTMotion>>>();
auto it = std::find(lines.begin(), lines.end(), "\t\tGeneralConstraintSets");
std::vector<std::string> motionsLines(lines.begin(), it);
std::shared_ptr<ASMTMotion> motion;
while (!motionsLines.empty()) {
if (motionsLines[0] == "\t\t\tRotationalMotion") {
motion = CREATE<ASMTRotationalMotion>::With();
}
else if (lines[0] == (leadingTabs + "Parts")) {
lines.erase(lines.begin());
parts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "KinematicIJs"));
std::vector<std::string> partsLines(lines.begin(), it);
while (!partsLines.empty()) {
if (partsLines[0] == (leadingTabs + "\tPart")) {
partsLines.erase(partsLines.begin());
auto part = CREATE<ASMTPart>::With();
part->parseASMT(partsLines);
parts->push_back(part);
part->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
else if (motionsLines[0] == "\t\t\tTranslationalMotion") {
motion = CREATE<ASMTTranslationalMotion>::With();
}
else if (lines[0] == (leadingTabs + "KinematicIJs")) {
lines.erase(lines.begin());
kinematicIJs = std::make_shared<std::vector<std::shared_ptr<ASMTKinematicIJ>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "ConstraintSets"));
std::vector<std::string> kinematicIJsLines(lines.begin(), it);
while (!kinematicIJsLines.empty()) {
if (kinematicIJsLines[0] == (leadingTabs + "\tKinematicIJ")) {
kinematicIJsLines.erase(kinematicIJsLines.begin());
auto kinematicIJ = CREATE<ASMTKinematicIJ>::With();
kinematicIJ->parseASMT(kinematicIJsLines);
kinematicIJs->push_back(kinematicIJ);
kinematicIJ->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
else if (motionsLines[0] == "\t\t\tGeneralMotion") {
motion = CREATE<ASMTGeneralMotion>::With();
}
else if (lines[0] == (leadingTabs + "ConstraintSets")) {
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "\tJoints"));
lines.erase(lines.begin());
joints = std::make_shared<std::vector<std::shared_ptr<ASMTJoint>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "\tMotions"));
std::vector<std::string> jointsLines(lines.begin(), it);
while (!jointsLines.empty()) {
if (jointsLines[0] == (leadingTabs + "\t\tRevoluteJoint")) {
jointsLines.erase(jointsLines.begin());
auto joint = CREATE<ASMTRevoluteJoint>::With();
joint->parseASMT(jointsLines);
joints->push_back(joint);
joint->owner = this;
}
else if (jointsLines[0] == (leadingTabs + "\t\tCylindricalJoint")) {
jointsLines.erase(jointsLines.begin());
auto joint = CREATE<ASMTCylindricalJoint>::With();
joint->parseASMT(jointsLines);
joints->push_back(joint);
joint->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
assert(lines[0] == (leadingTabs + "\tMotions"));
lines.erase(lines.begin());
motions = std::make_shared<std::vector<std::shared_ptr<ASMTMotion>>>();
it = std::find(lines.begin(), lines.end(), (leadingTabs + "\tGeneralConstraintSets"));
std::vector<std::string> motionsLines(lines.begin(), it);
while (!motionsLines.empty()) {
if (motionsLines[0] == (leadingTabs + "\t\tRotationalMotion")) {
motionsLines.erase(motionsLines.begin());
auto motion = CREATE<ASMTRotationalMotion>::With();
motion->parseASMT(motionsLines);
motions->push_back(motion);
motion->owner = this;
}
else if (motionsLines[0] == (leadingTabs + "\t\tTranslationalMotion")) {
motionsLines.erase(motionsLines.begin());
auto motion = CREATE<ASMTTranslationalMotion>::With();
motion->parseASMT(motionsLines);
motions->push_back(motion);
motion->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
assert(lines[0] == (leadingTabs + "\tGeneralConstraintSets"));
lines.erase(lines.begin());
constraintSets = std::make_shared<std::vector<std::shared_ptr<ASMTConstraintSet>>>();
it = std::find(lines.begin(), lines.end(), (leadingTabs + "ForceTorques"));
std::vector<std::string> generalConstraintSetsLines(lines.begin(), it);
while (!generalConstraintSetsLines.empty()) {
assert(false);
}
lines.erase(lines.begin(), it);
else {
assert(false);
}
else if (lines[0] == (leadingTabs + "ForceTorques")) {
lines.erase(lines.begin());
forceTorques = std::make_shared<std::vector<std::shared_ptr<ASMTForceTorque>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "ConstantGravity"));
std::vector<std::string> forceTorquesLines(lines.begin(), it);
while (!forceTorquesLines.empty()) {
if (forceTorquesLines[0] == (leadingTabs + "\tForceTorque")) {
forceTorquesLines.erase(forceTorquesLines.begin());
auto forceTorque = CREATE<ASMTForceTorque>::With();
forceTorque->parseASMT(forceTorquesLines);
forceTorques->push_back(forceTorque);
forceTorque->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
}
else if (lines[0] == (leadingTabs + "ConstantGravity")) {
lines.erase(lines.begin());
constantGravity = CREATE<ASMTConstantGravity>::With();
constantGravity->parseASMT(lines);
constantGravity->owner = this;
}
else if (lines[0] == (leadingTabs + "SimulationParameters")) {
lines.erase(lines.begin());
simulationParameters = CREATE<ASMTSimulationParameters>::With();
simulationParameters->parseASMT(lines);
simulationParameters->owner = this;
}
else if (lines[0] == (leadingTabs + "AnimationParameters")) {
lines.erase(lines.begin());
animationParameters = CREATE<ASMTAnimationParameters>::With();
animationParameters->parseASMT(lines);
animationParameters->owner = this;
motionsLines.erase(motionsLines.begin());
motion->parseASMT(motionsLines);
motions->push_back(motion);
motion->owner = this;
motion->initMarkers();
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTAssembly::readGeneralConstraintSets(std::vector<std::string>& lines)
{
assert(lines[0] == "\t\tGeneralConstraintSets");
lines.erase(lines.begin());
constraintSets = std::make_shared<std::vector<std::shared_ptr<ASMTConstraintSet>>>();
auto it = std::find(lines.begin(), lines.end(), "\tForceTorques");
std::vector<std::string> generalConstraintSetsLines(lines.begin(), it);
while (!generalConstraintSetsLines.empty()) {
assert(false);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTAssembly::readForcesTorques(std::vector<std::string>& lines)
{
assert(lines[0] == "\tForceTorques"); //Spelling is not consistent in asmt file.
lines.erase(lines.begin());
forcesTorques = std::make_shared<std::vector<std::shared_ptr<ASMTForceTorque>>>();
auto it = std::find(lines.begin(), lines.end(), "\tConstantGravity");
std::vector<std::string> forcesTorquesLines(lines.begin(), it);
while (!forcesTorquesLines.empty()) {
if (forcesTorquesLines[0] == "\t\tForceTorque") {
forcesTorquesLines.erase(forcesTorquesLines.begin());
auto forceTorque = CREATE<ASMTForceTorque>::With();
forceTorque->parseASMT(forcesTorquesLines);
forcesTorques->push_back(forceTorque);
forceTorque->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTAssembly::readConstantGravity(std::vector<std::string>& lines)
{
assert(lines[0] == "\tConstantGravity");
lines.erase(lines.begin());
constantGravity = CREATE<ASMTConstantGravity>::With();
constantGravity->parseASMT(lines);
constantGravity->owner = this;
}
void MbD::ASMTAssembly::readSimulationParameters(std::vector<std::string>& lines)
{
assert(lines[0] == "\tSimulationParameters");
lines.erase(lines.begin());
simulationParameters = CREATE<ASMTSimulationParameters>::With();
simulationParameters->parseASMT(lines);
simulationParameters->owner = this;
}
void MbD::ASMTAssembly::readAnimationParameters(std::vector<std::string>& lines)
{
assert(lines[0] == "\tAnimationParameters");
lines.erase(lines.begin());
animationParameters = CREATE<ASMTAnimationParameters>::With();
animationParameters->parseASMT(lines);
animationParameters->owner = this;
}
void MbD::ASMTAssembly::readTimeSeries(std::vector<std::string>& lines)
{
assert(lines[0] == "TimeSeries");
lines.erase(lines.begin());
assert(lines[0].find("Number\tInput") != std::string::npos);
lines.erase(lines.begin());
readTimes(lines);
}
void MbD::ASMTAssembly::readTimes(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "Time\tInput";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
times = readRowOfDoubles(str);
times->insert(times->begin(), times->at(0)); //The first element is the input state.
lines.erase(lines.begin());
}
void MbD::ASMTAssembly::readPartSeriesMany(std::vector<std::string>& lines)
{
assert(lines[0].find("PartSeries") != std::string::npos);
auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
return s.find("JointSeries") != std::string::npos;
});
std::vector<std::string> partSeriesLines(lines.begin(), it);
while (!partSeriesLines.empty()) {
readPartSeries(partSeriesLines);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTAssembly::readJointSeriesMany(std::vector<std::string>& lines)
{
assert(lines[0].find("JointSeries") != std::string::npos);
auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
return s.find("MotionSeries") != std::string::npos;
});
std::vector<std::string> jointSeriesLines(lines.begin(), it);
while (!jointSeriesLines.empty()) {
readJointSeries(jointSeriesLines);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTAssembly::readAssemblySeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "AssemblySeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
assert(fullName("") == seriesName);
lines.erase(lines.begin());
//xs, ys, zs, bryxs, bryys, bryzs
readXs(lines);
readYs(lines);
readZs(lines);
readBryantxs(lines);
readBryantys(lines);
readBryantzs(lines);
readVXs(lines);
readVYs(lines);
readVZs(lines);
readOmegaXs(lines);
readOmegaYs(lines);
readOmegaZs(lines);
readAXs(lines);
readAYs(lines);
readAZs(lines);
readAlphaXs(lines);
readAlphaYs(lines);
readAlphaZs(lines);
}
void MbD::ASMTAssembly::readPartSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "PartSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& prt) {
return prt->fullName("") == seriesName;
});
auto& part = *it;
part->readPartSeries(lines);
}
void MbD::ASMTAssembly::readJointSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "JointSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr<ASMTJoint>& jt) {
return jt->fullName("") == seriesName;
});
auto& joint = *it;
joint->readJointSeries(lines);
}
void MbD::ASMTAssembly::readMotionSeriesMany(std::vector<std::string>& lines)
{
while (!lines.empty()) {
assert(lines[0].find("MotionSeries") != std::string::npos);
readMotionSeries(lines);
}
}
void MbD::ASMTAssembly::readMotionSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "MotionSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr<ASMTMotion>& jt) {
return jt->fullName("") == seriesName;
});
auto& motion = *it;
motion->readMotionSeries(lines);
}
void MbD::ASMTAssembly::outputFor(AnalysisType type)
{
assert(false);
}
void MbD::ASMTAssembly::logString(std::string& str)
{
assert(false);
}
void MbD::ASMTAssembly::logString(double value)
{
assert(false);
}
void MbD::ASMTAssembly::preMbDrun(std::shared_ptr<System> mbdSys)
{
calcCharacteristicDimensions();
deleteMbD();
createMbD(mbdSys, mbdUnits);
std::static_pointer_cast<Part>(mbdObject)->asFixed();
}
void MbD::ASMTAssembly::postMbDrun()
{
assert(false);
}
void MbD::ASMTAssembly::calcCharacteristicDimensions()
{
auto unitTime = this->calcCharacteristicTime();
auto unitMass = this->calcCharacteristicMass();
auto unitLength = this->calcCharacteristicLength();
auto unitAngle = 1.0;
this->mbdUnits = std::make_shared<Units>(unitTime, unitMass, unitLength, unitAngle);
}
double MbD::ASMTAssembly::calcCharacteristicTime()
{
return std::abs(simulationParameters->hout);
}
double MbD::ASMTAssembly::calcCharacteristicMass()
{
auto n = parts->size();
double sumOfSquares = 0.0;
for (int i = 0; i < n; i++)
{
auto mass = parts->at(i)->principalMassMarker->mass;
sumOfSquares += mass * mass;
}
auto unitMass = std::sqrt(sumOfSquares / n);
if (unitMass <= 0) unitMass = 1.0;
return unitMass;
}
double MbD::ASMTAssembly::calcCharacteristicLength()
{
auto markerMap = this->markerMap();
auto lengths = std::make_shared<std::vector<double>>();
auto connectorList = this->connectorList();
for (auto& connector : *connectorList) {
auto& mkrI = markerMap->at(connector->markerI);
lengths->push_back(mkrI->rpmp()->length());
auto& mkrJ = markerMap->at(connector->markerJ);
lengths->push_back(mkrJ->rpmp()->length());
}
auto n = lengths->size();
double sumOfSquares = std::accumulate(lengths->begin(), lengths->end(), 0.0, [](double sum, double l) { return sum + l * l; });
auto unitLength = std::sqrt(sumOfSquares / std::max((int)n, 1));
if (unitLength <= 0) unitLength = 1.0;
return unitLength;
}
std::shared_ptr<std::vector<std::shared_ptr<ASMTItemIJ>>> MbD::ASMTAssembly::connectorList()
{
auto list = std::make_shared<std::vector<std::shared_ptr<ASMTItemIJ>>>();
list->insert(list->end(), joints->begin(), joints->end());
list->insert(list->end(), motions->begin(), motions->end());
list->insert(list->end(), kinematicIJs->begin(), kinematicIJs->end());
list->insert(list->end(), forcesTorques->begin(), forcesTorques->end());
return list;
}
std::shared_ptr<std::map<std::string, std::shared_ptr<ASMTMarker>>> MbD::ASMTAssembly::markerMap()
{
auto answer = std::make_shared<std::map<std::string, std::shared_ptr<ASMTMarker>>>();
for (auto& refPoint : *refPoints) {
for (auto& marker : *refPoint->markers) {
answer->insert(std::make_pair(marker->fullName(""), marker));
}
}
for (auto& part : *parts) {
for (auto& refPoint : *part->refPoints) {
for (auto& marker : *refPoint->markers) {
answer->insert(std::make_pair(marker->fullName(""), marker));
}
}
}
return answer;
}
void MbD::ASMTAssembly::deleteMbD()
{
ASMTSpatialContainer::deleteMbD();
constantGravity->deleteMbD();
asmtTime->deleteMbD();
for (auto& part : *parts) { part->deleteMbD(); }
for (auto& joint : *joints) { joint->deleteMbD(); }
for (auto& motion : *motions) { motion->deleteMbD(); }
for (auto& forceTorque : *forcesTorques) { forceTorque->deleteMbD(); }
}
void MbD::ASMTAssembly::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTSpatialContainer::createMbD(mbdSys, mbdUnits);
constantGravity->createMbD(mbdSys, mbdUnits);
asmtTime->createMbD(mbdSys, mbdUnits);
std::sort(parts->begin(), parts->end(), [](std::shared_ptr<ASMTPart> a, std::shared_ptr<ASMTPart> b) { return a->name < b->name; });
auto jointsMotions = std::make_shared<std::vector<std::shared_ptr<ASMTConstraintSet>>>();
jointsMotions->insert(jointsMotions->end(), joints->begin(), joints->end());
jointsMotions->insert(jointsMotions->end(), motions->begin(), motions->end());
std::sort(jointsMotions->begin(), jointsMotions->end(), [](std::shared_ptr<ASMTConstraintSet> a, std::shared_ptr<ASMTConstraintSet> b) { return a->name < b->name; });
std::sort(forcesTorques->begin(), forcesTorques->end(), [](std::shared_ptr<ASMTForceTorque> a, std::shared_ptr<ASMTForceTorque> b) { return a->name < b->name; });
for (auto& part : *parts) { part->createMbD(mbdSys, mbdUnits); }
for (auto& joint : *jointsMotions) { joint->createMbD(mbdSys, mbdUnits); }
for (auto& forceTorque : *forcesTorques) { forceTorque->createMbD(mbdSys, mbdUnits); }
auto mbdSysSolver = mbdSys->systemSolver;
mbdSysSolver->errorTolPosKine = simulationParameters->errorTolPosKine;
mbdSysSolver->errorTolAccKine = simulationParameters->errorTolAccKine;
mbdSysSolver->iterMaxPosKine = simulationParameters->iterMaxPosKine;
mbdSysSolver->iterMaxAccKine = simulationParameters->iterMaxAccKine;
mbdSysSolver->tstart = simulationParameters->tstart / mbdUnits->time;
mbdSysSolver->tend = simulationParameters->tend / mbdUnits->time;
mbdSysSolver->hmin = simulationParameters->hmin / mbdUnits->time;
mbdSysSolver->hmax = simulationParameters->hmax / mbdUnits->time;
mbdSysSolver->hout = simulationParameters->hout / mbdUnits->time;
mbdSysSolver->corAbsTol = simulationParameters->corAbsTol;
mbdSysSolver->corRelTol = simulationParameters->corRelTol;
mbdSysSolver->intAbsTol = simulationParameters->intAbsTol;
mbdSysSolver->intRelTol = simulationParameters->intRelTol;
mbdSysSolver->iterMaxDyn = simulationParameters->iterMaxDyn;
mbdSysSolver->orderMax = simulationParameters->orderMax;
mbdSysSolver->translationLimit = simulationParameters->translationLimit / mbdUnits->length;
mbdSysSolver->rotationLimit = simulationParameters->rotationLimit;
animationParameters = nullptr;
}
void MbD::ASMTAssembly::runKINEMATIC()
{
auto mbdSystem = CREATE<System>::With();
mbdObject = mbdSystem;
mbdSystem->externalSystem->asmtAssembly = this;
mbdSystem->runKINEMATIC(mbdSystem);
}
//
//void MbD::ASMTAssembly::initprincipalMassMarker()
//{
// //Choose first refPoint as center of mass
// assert(!refPoints->empty());
// auto refPoint = refPoints->at(0);
// principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
// principalMassMarker->position3D = refPoint->position3D;
// principalMassMarker->rotationMatrix = refPoint->rotationMatrix;
//}
void MbD::ASMTAssembly::initprincipalMassMarker()
{
principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
principalMassMarker->mass = 0.0;
principalMassMarker->density = 0.0;
principalMassMarker->momentOfInertias = std::make_shared<DiagonalMatrix<double>>(3, 0);
principalMassMarker->position3D = std::make_shared<FullColumn<double>>(3, 0);
principalMassMarker->rotationMatrix = std::make_shared<FullMatrix<double>>(3, 3);
principalMassMarker->rotationMatrix->identity();
}
std::shared_ptr<ASMTSpatialContainer> MbD::ASMTAssembly::spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname)
{
if ((self->fullName("")) == longname) return self;
auto it = std::find_if(parts->begin(), parts->end(), [&](const std::shared_ptr<ASMTPart>& prt) {
return prt->fullName("") == longname;
});
auto& part = *it;
return part;
}
std::shared_ptr<ASMTMarker> MbD::ASMTAssembly::markerAt(std::string& longname)
{
for (auto& refPoint : *refPoints) {
for (auto& marker : *refPoint->markers) {
if (marker->fullName("") == longname) return marker;
}
}
for (auto& part : *parts) {
for (auto& refPoint : *part->refPoints) {
for (auto& marker : *refPoint->markers) {
if (marker->fullName("") == longname) return marker;
}
}
}
return nullptr;
}
std::shared_ptr<ASMTJoint> MbD::ASMTAssembly::jointAt(std::string& longname)
{
auto it = std::find_if(joints->begin(), joints->end(), [&](const std::shared_ptr<ASMTJoint>& jt) {
return jt->fullName("") == longname;
});
auto& joint = *it;
return joint;
}
std::shared_ptr<ASMTMotion> MbD::ASMTAssembly::motionAt(std::string& longname)
{
auto it = std::find_if(motions->begin(), motions->end(), [&](const std::shared_ptr<ASMTMotion>& mt) {
return mt->fullName("") == longname;
});
auto& motion = *it;
return motion;
}
std::shared_ptr<ASMTForceTorque> MbD::ASMTAssembly::forceTorqueAt(std::string& longname)
{
auto it = std::find_if(forcesTorques->begin(), forcesTorques->end(), [&](const std::shared_ptr<ASMTForceTorque>& mt) {
return mt->fullName("") == longname;
});
auto& forceTorque = *it;
return forceTorque;
}
FColDsptr MbD::ASMTAssembly::vOcmO()
{
return std::make_shared<FullColumn<double>>(3, 0.0);
}
FColDsptr MbD::ASMTAssembly::omeOpO()
{
return std::make_shared<FullColumn<double>>(3, 0.0);
}
std::shared_ptr<ASMTTime> MbD::ASMTAssembly::geoTime()
{
return asmtTime;
}
void MbD::ASMTAssembly::updateFromMbD()
{
ASMTSpatialContainer::updateFromMbD();
auto geoTime = asmtTime->getValue();
auto it = std::find_if(times->begin(), times->end(), [&](double t) {
return Numeric::equaltol(t, geoTime, 1.0e-9);
});
assert(it != times->end());
for (auto& part : *parts) part->updateFromMbD();
for (auto& joint : *joints) joint->updateFromMbD();
for (auto& motion : *motions) motion->updateFromMbD();
for (auto& forceTorque : *forcesTorques) forceTorque->updateFromMbD();
}
void MbD::ASMTAssembly::compareResults(AnalysisType type)
{
ASMTSpatialContainer::compareResults(type);
for (auto& part : *parts) part->compareResults(type);
for (auto& joint : *joints) joint->compareResults(type);
for (auto& motion : *motions) motion->compareResults(type);
for (auto& forceTorque : *forcesTorques) forceTorque->compareResults(type);
}

76
MbDCode/ASMTAssembly.h
View File

@@ -1,7 +1,15 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include <fstream>
#include "ASMTItem.h"
#include "ASMTSpatialContainer.h"
#include "ASMTRefPoint.h"
#include "ASMTRefCurve.h"
#include "ASMTRefSurface.h"
@@ -16,30 +24,80 @@
#include "FullMatrix.h"
#include "ASMTJoint.h"
#include "ASMTMotion.h"
#include "Units.h"
#include "ASMTTime.h"
#include "SystemSolver.h"
namespace MbD {
class ASMTAssembly : public ASMTItem
class ASMTAssembly : public ASMTSpatialContainer
{
//
public:
static void runFile(const char* chars);
ASMTAssembly* root() override;
void parseASMT(std::vector<std::string>& lines) override;
void readNotes(std::vector<std::string>& lines);
void readParts(std::vector<std::string>& lines);
void readPart(std::vector<std::string>& lines);
void readKinematicIJs(std::vector<std::string>& lines);
void readKinematicIJ(std::vector<std::string>& lines);
void readConstraintSets(std::vector<std::string>& lines);
void readJoints(std::vector<std::string>& lines);
void readMotions(std::vector<std::string>& lines);
void readGeneralConstraintSets(std::vector<std::string>& lines);
void readForcesTorques(std::vector<std::string>& lines);
void readConstantGravity(std::vector<std::string>& lines);
void readSimulationParameters(std::vector<std::string>& lines);
void readAnimationParameters(std::vector<std::string>& lines);
void readTimeSeries(std::vector<std::string>& lines);
void readTimes(std::vector<std::string>& lines);
void readAssemblySeries(std::vector<std::string>& lines);
void readPartSeriesMany(std::vector<std::string>& lines);
void readPartSeries(std::vector<std::string>& lines);
void readJointSeriesMany(std::vector<std::string>& lines);
void readJointSeries(std::vector<std::string>& lines);
void readMotionSeriesMany(std::vector<std::string>& lines);
void readMotionSeries(std::vector<std::string>& lines);
std::string notes, name;
FColDsptr position3D, velocity3D, omega3D;
FMatDsptr rotationMatrix;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefSurface>>> refSurfaces;
void outputFor(AnalysisType type);
void logString(std::string& str);
void logString(double value);
void preMbDrun(std::shared_ptr<System> mbdSys);
void postMbDrun();
void calcCharacteristicDimensions();
double calcCharacteristicTime();
double calcCharacteristicMass();
double calcCharacteristicLength();
std::shared_ptr<std::vector<std::shared_ptr<ASMTItemIJ>>> connectorList();
std::shared_ptr<std::map<std::string, std::shared_ptr<ASMTMarker>>>markerMap();
void deleteMbD();
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
void runKINEMATIC();
void initprincipalMassMarker();
std::shared_ptr<ASMTSpatialContainer> spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname);
std::shared_ptr<ASMTMarker> markerAt(std::string& longname);
std::shared_ptr<ASMTJoint> jointAt(std::string& longname);
std::shared_ptr<ASMTMotion> motionAt(std::string& longname);
std::shared_ptr<ASMTForceTorque> forceTorqueAt(std::string& longname);
FColDsptr vOcmO() override;
FColDsptr omeOpO() override;
std::shared_ptr<ASMTTime> geoTime();
void updateFromMbD() override;
void compareResults(AnalysisType type) override;
std::string notes;
std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> parts;
std::shared_ptr<std::vector<std::shared_ptr<ASMTKinematicIJ>>> kinematicIJs;
std::shared_ptr<std::vector<std::shared_ptr<ASMTConstraintSet>>> constraintSets;
std::shared_ptr<std::vector<std::shared_ptr<ASMTJoint>>> joints;
std::shared_ptr<std::vector<std::shared_ptr<ASMTMotion>>> motions;
std::shared_ptr<std::vector<std::shared_ptr<ASMTForceTorque>>> forceTorques;
std::shared_ptr<std::vector<std::shared_ptr<ASMTForceTorque>>> forcesTorques;
std::shared_ptr<ASMTConstantGravity> constantGravity;
std::shared_ptr<ASMTSimulationParameters> simulationParameters;
std::shared_ptr<ASMTAnimationParameters> animationParameters;
std::shared_ptr<std::vector<double>> times;
std::shared_ptr<ASMTTime> asmtTime = std::make_shared<ASMTTime>();
std::shared_ptr<Units> mbdUnits;
};
}

21
MbDCode/ASMTConstantGravity.cpp
View File

@@ -1,4 +1,17 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTConstantGravity.h"
#include "ASMTAssembly.h"
#include "Units.h"
#include "ConstantGravity.h"
#include "System.h"
#include "Part.h"
using namespace MbD;
@@ -7,3 +20,11 @@ void MbD::ASMTConstantGravity::parseASMT(std::vector<std::string>& lines)
g = readColumnOfDoubles(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTConstantGravity::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mbdGravity = CREATE<ConstantGravity>::With();
mbdObject = mbdGravity;
mbdGravity->gXYZ = g->times(1.0 / mbdUnits->acceleration);
mbdSys->addForceTorque(mbdGravity);
}

13
MbDCode/ASMTConstantGravity.h
View File

@@ -1,13 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
//#include "Units.h"
namespace MbD {
class System;
class Units;
class ASMTConstantGravity : public ASMTItem
{
//
public:
void parseASMT(std::vector<std::string>& lines) override;
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
FColDsptr g;
};

69
MbDCode/ASMTConstraintSet.cpp
View File

@@ -1,3 +1,72 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTConstraintSet.h"
#include "ASMTAssembly.h"
#include "ASMTMarker.h"
#include "Joint.h"
using namespace MbD;
void MbD::ASMTConstraintSet::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
//self dataSeries : OrderedCollection new.
//self discontinuities : OrderedCollection new.
auto mbdJt = this->mbdClassNew();
mbdObject = mbdJt;
mbdJt->name = fullName("");
auto mrkI = std::static_pointer_cast<EndFramec>(root()->markerAt(markerI)->mbdObject);
auto mrkJ = std::static_pointer_cast<EndFramec>(root()->markerAt(markerJ)->mbdObject);
mbdJt->connectsItoJ(mrkI, mrkJ);
mbdSys->addJoint(mbdJt);
}
std::shared_ptr<Joint> MbD::ASMTConstraintSet::mbdClassNew()
{
assert(false);
return std::shared_ptr<Joint>();
}
void MbD::ASMTConstraintSet::updateFromMbD()
{
//"
//MbD returns aFIeO and aTIeO.
//GEO needs aFImO and aTImO.
//For Motion rImIeO is not zero and is changing.
//aFImO = aFIeO.
//aTImO = aTIeO + (rImIeO cross : aFIeO).
//"
auto mbdUnts = mbdUnits();
auto mbdJoint = std::static_pointer_cast<Joint>(mbdObject);
auto aFIeO = mbdJoint->aFX()->times(mbdUnts->force);
auto aTIeO = mbdJoint->aTX()->times(mbdUnts->torque);
auto rImIeO = mbdJoint->frmI->rmeO()->times(mbdUnts->length);
auto aFIO = aFIeO;
auto aTIO = aTIeO->plusFullColumn(rImIeO->cross(aFIeO));
fxs->push_back(aFIO->at(0));
fys->push_back(aFIO->at(1));
fzs->push_back(aFIO->at(2));
txs->push_back(aTIO->at(0));
tys->push_back(aTIO->at(1));
tzs->push_back(aTIO->at(2));
}
void MbD::ASMTConstraintSet::compareResults(AnalysisType type)
{
auto mbdUnts = mbdUnits();
auto factor = 1.0e-6;
auto forceTol = mbdUnts->force * factor;
auto torqueTol = mbdUnts->torque * factor;
auto i = fxs->size() - 1;
//assert(Numeric::equaltol(fxs->at(i), infxs->at(i), forceTol));
//assert(Numeric::equaltol(fys->at(i), infys->at(i), forceTol));
//assert(Numeric::equaltol(fzs->at(i), infzs->at(i), forceTol));
//assert(Numeric::equaltol(txs->at(i), intxs->at(i), torqueTol));
//assert(Numeric::equaltol(tys->at(i), intys->at(i), torqueTol));
//assert(Numeric::equaltol(tzs->at(i), intzs->at(i), torqueTol));
}

19
MbDCode/ASMTConstraintSet.h
View File

@@ -1,13 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
#include "ASMTItemIJ.h"
namespace MbD {
class ASMTConstraintSet : public ASMTItem
class Joint;
class ASMTConstraintSet : public ASMTItemIJ
{
//
public:
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
virtual std::shared_ptr<Joint> mbdClassNew();
void updateFromMbD() override;
void compareResults(AnalysisType type) override;
};
}

12
MbDCode/ASMTCylindricalJoint.cpp
View File

@@ -1,4 +1,16 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTCylindricalJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTCylindricalJoint::mbdClassNew()
{
return CREATE<CylindricalJoint>::With();
}

10
MbDCode/ASMTCylindricalJoint.h
View File

@@ -1,12 +1,22 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTJoint.h"
#include "CylindricalJoint.h"
namespace MbD {
class ASMTCylindricalJoint : public ASMTJoint
{
//
public:
std::shared_ptr<Joint> mbdClassNew() override;
};

8
MbDCode/ASMTExtrusion.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTExtrusion.h"
using namespace MbD;

8
MbDCode/ASMTExtrusion.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"

16
MbDCode/ASMTFixedJoint.cpp Normal file
View File

@@ -0,0 +1,16 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTFixedJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTFixedJoint::mbdClassNew()
{
return CREATE<FixedJoint>::With();
}

22
MbDCode/ASMTFixedJoint.h Normal file
View File

@@ -0,0 +1,22 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTJoint.h"
#include "FixedJoint.h"
namespace MbD {
class ASMTFixedJoint : public ASMTJoint
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
};
}

18
MbDCode/ASMTForceTorque.cpp
View File

@@ -1,3 +1,21 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTForceTorque.h"
using namespace MbD;
void MbD::ASMTForceTorque::updateFromMbD()
{
assert(false);
}
void MbD::ASMTForceTorque::compareResults(AnalysisType type)
{
assert(false);
}

14
MbDCode/ASMTForceTorque.h
View File

@@ -1,12 +1,22 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
#include "ASMTItemIJ.h"
namespace MbD {
class ASMTForceTorque : public ASMTItem
class ASMTForceTorque : public ASMTItemIJ
{
//
public:
void updateFromMbD() override;
void compareResults(AnalysisType type) override;
};

140
MbDCode/ASMTGeneralMotion.cpp
View File

@@ -1,7 +1,147 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTGeneralMotion.h"
#include "ASMTAssembly.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "CREATE.h"
#include "Constant.h"
#include "EulerAngles.h"
using namespace MbD;
void MbD::ASMTGeneralMotion::parseASMT(std::vector<std::string>& lines)
{
readName(lines);
readMarkerI(lines);
readMarkerJ(lines);
readrIJI(lines);
readangIJJ(lines);
readRotationOrder(lines);
}
void MbD::ASMTGeneralMotion::readrIJI(std::vector<std::string>& lines)
{
rIJI = std::make_shared<std::vector<std::string>>(3);
assert(lines[0].find("rIJI1") != std::string::npos);
lines.erase(lines.begin());
rIJI->at(0) = readString(lines[0]);
lines.erase(lines.begin());
assert(lines[0].find("rIJI2") != std::string::npos);
lines.erase(lines.begin());
rIJI->at(1) = readString(lines[0]);
lines.erase(lines.begin());
assert(lines[0].find("rIJI3") != std::string::npos);
lines.erase(lines.begin());
rIJI->at(2) = readString(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTGeneralMotion::readangIJJ(std::vector<std::string>& lines)
{
angIJJ = std::make_shared<std::vector<std::string>>(3);
assert(lines[0].find("angIJJ1") != std::string::npos);
lines.erase(lines.begin());
angIJJ->at(0) = readString(lines[0]);
lines.erase(lines.begin());
assert(lines[0].find("angIJJ2") != std::string::npos);
lines.erase(lines.begin());
angIJJ->at(1) = readString(lines[0]);
lines.erase(lines.begin());
assert(lines[0].find("angIJJ3") != std::string::npos);
lines.erase(lines.begin());
angIJJ->at(2) = readString(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTGeneralMotion::readRotationOrder(std::vector<std::string>& lines)
{
assert(lines[0].find("RotationOrder") != std::string::npos);
lines.erase(lines.begin());
rotationOrder = readString(lines[0]);
lines.erase(lines.begin());
}
std::shared_ptr<Joint> MbD::ASMTGeneralMotion::mbdClassNew()
{
return CREATE<FullMotion>::With();
}
void MbD::ASMTGeneralMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTMotion::createMbD(mbdSys, mbdUnits);
auto parser = CREATE<SymbolicParser>::With();
parser->owner = this;
auto geoTime = owner->root()->geoTime();
parser->variables->insert(std::make_pair("time", geoTime));
std::shared_ptr< BasicUserFunction> userFunc;
auto fullMotion = std::static_pointer_cast<FullMotion>(mbdObject);
//rIJI
userFunc = CREATE<BasicUserFunction>::With(rIJI->at(0), 1.0);
parser->parseUserFunction(userFunc);
auto geoX = parser->stack->top();
geoX = Symbolic::times(geoX, std::make_shared<Constant>(1.0 / mbdUnits->length));
geoX->createMbD(mbdSys, mbdUnits);
auto xBlk = geoX->simplified(geoX);
userFunc = CREATE<BasicUserFunction>::With(rIJI->at(1), 1.0);
parser->parseUserFunction(userFunc);
auto geoY = parser->stack->top();
geoY = Symbolic::times(geoY, std::make_shared<Constant>(1.0 / mbdUnits->length));
geoY->createMbD(mbdSys, mbdUnits);
auto yBlk = geoY->simplified(geoY);
userFunc = CREATE<BasicUserFunction>::With(rIJI->at(2), 1.0);
parser->parseUserFunction(userFunc);
auto geoZ = parser->stack->top();
geoZ = Symbolic::times(geoZ, std::make_shared<Constant>(1.0 / mbdUnits->length));
geoZ->createMbD(mbdSys, mbdUnits);
auto zBlk = geoZ->simplified(geoZ);
auto xyzBlkList = std::initializer_list<Symsptr>{ xBlk, yBlk, zBlk };
fullMotion->frIJI = std::make_shared<FullColumn<Symsptr>>(xyzBlkList);
//angIJJ
userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(0), 1.0);
parser->parseUserFunction(userFunc);
auto geoPhi = parser->stack->top();
geoPhi = Symbolic::times(geoPhi, std::make_shared<Constant>(1.0 / mbdUnits->angle));
geoPhi->createMbD(mbdSys, mbdUnits);
auto phiBlk = geoPhi->simplified(geoPhi);
userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(1), 1.0);
parser->parseUserFunction(userFunc);
auto geoThe = parser->stack->top();
geoThe = Symbolic::times(geoThe, std::make_shared<Constant>(1.0 / mbdUnits->angle));
geoThe->createMbD(mbdSys, mbdUnits);
auto theBlk = geoThe->simplified(geoThe);
userFunc = CREATE<BasicUserFunction>::With(angIJJ->at(2), 1.0);
parser->parseUserFunction(userFunc);
auto geoPsi = parser->stack->top();
geoPsi = Symbolic::times(geoPsi, std::make_shared<Constant>(1.0 / mbdUnits->angle));
geoPsi->createMbD(mbdSys, mbdUnits);
auto psiBlk = geoPsi->simplified(geoPsi);
auto xyzRotBlkList = std::initializer_list<Symsptr>{ phiBlk, theBlk, psiBlk };
auto fangIJJ = std::make_shared<EulerAngles<Symsptr>>(xyzRotBlkList);
std::istringstream iss(rotationOrder);
auto rotOrder = std::make_shared<FullColumn<int>>();
int order;
for (int i = 0; i < 3; i++)
{
iss >> order;
rotOrder->push_back(order);
}
fangIJJ->rotOrder = rotOrder;
fullMotion->fangIJJ = fangIJJ;
}

16
MbDCode/ASMTGeneralMotion.h
View File

@@ -1,6 +1,15 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTMotion.h"
#include "FullMotion.h"
namespace MbD {
class ASMTGeneralMotion : public ASMTMotion
@@ -8,7 +17,14 @@ namespace MbD {
//
public:
void parseASMT(std::vector<std::string>& lines) override;
void readrIJI(std::vector<std::string>& lines);
void readangIJJ(std::vector<std::string>& lines);
void readRotationOrder(std::vector<std::string>& lines);
std::shared_ptr<Joint> mbdClassNew() override;
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
std::shared_ptr<std::vector<std::string>> rIJI, angIJJ;
std::string rotationOrder;
};
}

99
MbDCode/ASMTItem.cpp
View File

@@ -1,8 +1,29 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTItem.h"
#include "CREATE.h"
#include "ASMTSpatialContainer.h"
#include "ASMTAssembly.h"
#include "Constant.h"
using namespace MbD;
ASMTAssembly* MbD::ASMTItem::root()
{
return owner->root();
}
std::shared_ptr<ASMTSpatialContainer> MbD::ASMTItem::part()
{
return owner->part();
}
void MbD::ASMTItem::initialize()
{
}
@@ -12,7 +33,7 @@ void MbD::ASMTItem::parseASMT(std::vector<std::string>& lines)
assert(false);
}
FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string line)
FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string& line)
{
std::istringstream iss(line);
auto readRowOfDoubles = std::make_shared<FullRow<double>>();
@@ -23,7 +44,7 @@ FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string line)
return readRowOfDoubles;
}
FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string line)
FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string& line)
{
std::istringstream iss(line);
auto readColumnOfDoubles = std::make_shared<FullColumn<double>>();
@@ -34,7 +55,7 @@ FColDsptr MbD::ASMTItem::readColumnOfDoubles(std::string line)
return readColumnOfDoubles;
}
double MbD::ASMTItem::readDouble(std::string line)
double MbD::ASMTItem::readDouble(std::string& line)
{
std::istringstream iss(line);
double d;
@@ -42,7 +63,7 @@ double MbD::ASMTItem::readDouble(std::string line)
return d;
}
int MbD::ASMTItem::readInt(std::string line)
int MbD::ASMTItem::readInt(std::string& line)
{
std::istringstream iss(line);
int i;
@@ -50,7 +71,7 @@ int MbD::ASMTItem::readInt(std::string line)
return i;
}
bool MbD::ASMTItem::readBool(std::string line)
bool MbD::ASMTItem::readBool(std::string& line)
{
if (line.find("true") != std::string::npos)
{
@@ -62,5 +83,73 @@ bool MbD::ASMTItem::readBool(std::string line)
}
else {
assert(false);
return false;
}
}
std::string MbD::ASMTItem::readString(std::string& line)
{
std::string str = line;
str.erase(str.begin(), std::find_if(str.begin(), str.end(), [](unsigned char ch) { return !std::isspace(ch); }));
return str;
}
void MbD::ASMTItem::readName(std::vector<std::string>& lines)
{
assert(lines[0].find("Name") != std::string::npos);
lines.erase(lines.begin());
name = readString(lines[0]);
lines.erase(lines.begin());
}
std::string MbD::ASMTItem::fullName(std::string partialName)
{
std::string longerName = "/" + name + partialName;
if (owner == nullptr) {
return longerName;
}
else {
return owner->fullName(longerName);
}
}
void MbD::ASMTItem::readDoublesInto(std::string& str, std::string label, FRowDsptr& row)
{
auto pos = str.find(label);
assert(pos != std::string::npos);
str.erase(0, pos + label.length());
row = readRowOfDoubles(str);
}
void MbD::ASMTItem::deleteMbD()
{
mbdObject = nullptr;
}
void MbD::ASMTItem::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
assert(false);
}
void MbD::ASMTItem::updateFromMbD()
{
assert(false);
}
void MbD::ASMTItem::compareResults(AnalysisType type)
{
assert(false);
}
std::shared_ptr<Units> MbD::ASMTItem::mbdUnits()
{
if (owner) {
return owner->mbdUnits();
}
return static_cast<ASMTAssembly*>(this)->mbdUnits;
}
std::shared_ptr<Constant> MbD::ASMTItem::sptrConstant(double value)
{
return std::make_shared<Constant>(value);
}

37
MbDCode/ASMTItem.h
View File

@@ -1,20 +1,47 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "CREATE.h"
namespace MbD {
class ASMTAssembly;
class Units;
class ASMTSpatialContainer;
class ASMTItem
{
//
public:
virtual ASMTAssembly* root();
virtual std::shared_ptr<ASMTSpatialContainer> part();
virtual void initialize();
virtual void parseASMT(std::vector<std::string>& lines);
FRowDsptr readRowOfDoubles(std::string line);
FColDsptr readColumnOfDoubles(std::string line);
double readDouble(std::string line);
int readInt(std::string line);
bool readBool(std::string line);
FRowDsptr readRowOfDoubles(std::string& line);
FColDsptr readColumnOfDoubles(std::string& line);
double readDouble(std::string& line);
int readInt(std::string& line);
bool readBool(std::string& line);
std::string readString(std::string& line);
void readName(std::vector<std::string>& lines);
virtual std::string fullName(std::string partialName);
void readDoublesInto(std::string& str, std::string label, FRowDsptr& row);
virtual void deleteMbD();
virtual void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits);
virtual void updateFromMbD();
virtual void compareResults(AnalysisType type);
std::shared_ptr<Units> mbdUnits();
std::shared_ptr<Constant> sptrConstant(double value);
std::string name;
ASMTItem* owner;
std::shared_ptr<Item> mbdObject;
};
}

77
MbDCode/ASMTItemIJ.cpp Normal file
View File

@@ -0,0 +1,77 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTItemIJ.h"
void MbD::ASMTItemIJ::initialize()
{
fxs = std::make_shared<FullRow<double>>();
fys = std::make_shared<FullRow<double>>();
fzs = std::make_shared<FullRow<double>>();
txs = std::make_shared<FullRow<double>>();
tys = std::make_shared<FullRow<double>>();
tzs = std::make_shared<FullRow<double>>();
}
void MbD::ASMTItemIJ::readMarkerI(std::vector<std::string>& lines)
{
assert(lines[0].find("MarkerI") != std::string::npos);
lines.erase(lines.begin());
markerI = readString(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTItemIJ::readMarkerJ(std::vector<std::string>& lines)
{
assert(lines[0].find("MarkerJ") != std::string::npos);
lines.erase(lines.begin());
markerJ = readString(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTItemIJ::readFXonIs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "FXonI", infxs);
lines.erase(lines.begin());
}
void MbD::ASMTItemIJ::readFYonIs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "FYonI", infys);
lines.erase(lines.begin());
}
void MbD::ASMTItemIJ::readFZonIs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "FZonI", infzs);
lines.erase(lines.begin());
}
void MbD::ASMTItemIJ::readTXonIs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "TXonI", intxs);
lines.erase(lines.begin());
}
void MbD::ASMTItemIJ::readTYonIs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "TYonI", intys);
lines.erase(lines.begin());
}
void MbD::ASMTItemIJ::readTZonIs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "TZonI", intzs);
lines.erase(lines.begin());
}

34
MbDCode/ASMTItemIJ.h Normal file
View File

@@ -0,0 +1,34 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
namespace MbD {
class ASMTItemIJ : public ASMTItem
{
//
public:
void initialize() override;
void readMarkerI(std::vector<std::string>& lines);
void readMarkerJ(std::vector<std::string>& lines);
void readFXonIs(std::vector<std::string>& lines);
void readFYonIs(std::vector<std::string>& lines);
void readFZonIs(std::vector<std::string>& lines);
void readTXonIs(std::vector<std::string>& lines);
void readTYonIs(std::vector<std::string>& lines);
void readTZonIs(std::vector<std::string>& lines);
std::string markerI, markerJ;
FRowDsptr fxs, fys, fzs, txs, tys, tzs;
FRowDsptr infxs, infys, infzs, intxs, intys, intzs;
};
}

43
MbDCode/ASMTJoint.cpp
View File

@@ -1,21 +1,36 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTJoint.h"
using namespace MbD;
void MbD::ASMTJoint::parseASMT(std::vector<std::string>& lines)
{
size_t pos = lines[0].find_first_not_of("\t");
auto leadingTabs = lines[0].substr(0, pos);
assert(lines[0] == (leadingTabs + "Name"));
lines.erase(lines.begin());
name = lines[0];
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "MarkerI"));
lines.erase(lines.begin());
markerI = lines[0];
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "MarkerJ"));
lines.erase(lines.begin());
markerJ = lines[0];
lines.erase(lines.begin());
readName(lines);
readMarkerI(lines);
readMarkerJ(lines);
}
void MbD::ASMTJoint::readJointSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "JointSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
assert(fullName("") == seriesName);
lines.erase(lines.begin());
readFXonIs(lines);
readFYonIs(lines);
readFZonIs(lines);
readTXonIs(lines);
readTYonIs(lines);
readTZonIs(lines);
}

12
MbDCode/ASMTJoint.h
View File

@@ -1,6 +1,15 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTConstraintSet.h"
#include "ForceTorqueData.h"
namespace MbD {
class ASMTJoint : public ASMTConstraintSet
@@ -8,8 +17,9 @@ namespace MbD {
//
public:
void parseASMT(std::vector<std::string>& lines) override;
void readJointSeries(std::vector<std::string>& lines);
std::string name, markerI, markerJ;
std::shared_ptr<std::vector<std::shared_ptr<ForceTorqueData>>> jointSeries;
};
}

8
MbDCode/ASMTKinematicIJ.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTKinematicIJ.h"
using namespace MbD;

12
MbDCode/ASMTKinematicIJ.h
View File

@@ -1,9 +1,17 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
#include "ASMTItemIJ.h"
namespace MbD {
class ASMTKinematicIJ : public ASMTItem
class ASMTKinematicIJ : public ASMTItemIJ
{
//
public:

90
MbDCode/ASMTMarker.cpp
View File

@@ -1,43 +1,61 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTMarker.h"
#include "ASMTRefItem.h"
#include "ASMTPart.h"
#include "Part.h"
#include "PartFrame.h"
#include "MarkerFrame.h"
using namespace MbD;
void MbD::ASMTMarker::parseASMT(std::vector<std::string>& lines)
{
size_t pos = lines[0].find_first_not_of("\t");
auto leadingTabs = lines[0].substr(0, pos);
while (!lines.empty()) {
if (lines[0] == (leadingTabs + "Name")) {
lines.erase(lines.begin());
name = lines[0];
lines.erase(lines.begin());
}
else if (lines[0] == (leadingTabs + "Position3D")) {
lines.erase(lines.begin());
std::istringstream iss(lines[0]);
position3D = std::make_shared<FullColumn<double>>();
double d;
while (iss >> d) {
position3D->push_back(d);
}
lines.erase(lines.begin());
}
else if (lines[0] == (leadingTabs + "RotationMatrix")) {
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
for (int i = 0; i < 3; i++)
{
auto& row = rotationMatrix->at(i);
std::istringstream iss(lines[0]);
double d;
while (iss >> d) {
row->push_back(d);
}
lines.erase(lines.begin());
}
}
else {
assert(false);
}
}
readName(lines);
readPosition3D(lines);
readRotationMatrix(lines);
}
FColDsptr MbD::ASMTMarker::rpmp()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& rPrefP = refItem->position3D;
auto& aAPref = refItem->rotationMatrix;
auto& rrefmref = position3D;
auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& rPcmP = principalMassMarker->position3D;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
return rpmp;
}
FMatDsptr MbD::ASMTMarker::aApm()
{
//p is cm
auto refItem = static_cast<ASMTRefItem*>(owner);
auto& aAPref = refItem->rotationMatrix;
auto& aArefm = rotationMatrix;
auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
return aApm;
}
void MbD::ASMTMarker::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mkr = CREATE<MarkerFrame>::With(name.c_str());
auto prt = std::static_pointer_cast<Part>(part()->mbdObject);
prt->partFrame->addMarkerFrame(mkr);
mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
mkr->aApm = aApm();
mbdObject = mkr->endFrames->at(0);
}

19
MbDCode/ASMTMarker.h
View File

@@ -1,19 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
#include "ASMTSpatialItem.h"
#include "FullColumn.h"
#include "FullMatrix.h"
namespace MbD {
class ASMTMarker : public ASMTItem
class ASMTMarker : public ASMTSpatialItem
{
//
public:
void parseASMT(std::vector<std::string>& lines) override;
std::string name;
FColDsptr position3D;
FMatDsptr rotationMatrix;
FColDsptr rpmp();
FMatDsptr aApm();
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
};
}

30
MbDCode/ASMTMotion.cpp
View File

@@ -1,3 +1,33 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTMotion.h"
using namespace MbD;
void MbD::ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "MotionSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
assert(fullName("") == seriesName);
lines.erase(lines.begin());
readFXonIs(lines);
readFYonIs(lines);
readFZonIs(lines);
readTXonIs(lines);
readTYonIs(lines);
readTZonIs(lines);
}
void MbD::ASMTMotion::initMarkers()
{
}

12
MbDCode/ASMTMotion.h
View File

@@ -1,13 +1,25 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTConstraintSet.h"
#include "ForceTorqueData.h"
namespace MbD {
class ASMTMotion : public ASMTConstraintSet
{
//
public:
void readMotionSeries(std::vector<std::string>& lines);
virtual void initMarkers();
std::shared_ptr<std::vector<std::shared_ptr<ForceTorqueData>>> motionSeries;
};
}

215
MbDCode/ASMTPart.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTPart.h"
#include "CREATE.h"
@@ -5,123 +13,92 @@ using namespace MbD;
void MbD::ASMTPart::parseASMT(std::vector<std::string>& lines)
{
size_t pos = lines[0].find_first_not_of("\t");
auto leadingTabs = lines[0].substr(0, pos);
while (!lines.empty()) {
if (lines[0] == (leadingTabs + "Name")) {
lines.erase(lines.begin());
name = lines[0];
lines.erase(lines.begin());
}
else if (lines[0] == (leadingTabs + "Position3D")) {
lines.erase(lines.begin());
position3D = readColumnOfDoubles(lines[0]);
lines.erase(lines.begin());
}
else if (lines[0] == (leadingTabs + "RotationMatrix")) {
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3);
for (int i = 0; i < 3; i++)
{
auto row = readRowOfDoubles(lines[0]);
rotationMatrix->atiput(i, row);
lines.erase(lines.begin());
}
}
else if (lines[0] == (leadingTabs + "Velocity3D")) {
lines.erase(lines.begin());
velocity3D = readColumnOfDoubles(lines[0]);
lines.erase(lines.begin());
}
else if (lines[0] == (leadingTabs + "Omega3D")) {
lines.erase(lines.begin());
omega3D = readColumnOfDoubles(lines[0]);
lines.erase(lines.begin());
}
else if (lines[0] == (leadingTabs + "FeatureOrder")) {
lines.erase(lines.begin());
//featureOrder = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "PrincipalMassMarker"));
//std::vector<std::string> featureOrderLines(lines.begin(), it);
//while (!featureOrderLines.empty()) {
// if (featureOrderLines[0] == (leadingTabs + "\tExtrusion")) {
// featureOrderLines.erase(featureOrderLines.begin());
// auto extrusion = CREATE<ASMTExtrusion>::With();
// extrusion->parseASMT(featureOrderLines);
// featureOrder->push_back(extrusion);
// extrusion->owner = this;
// }
// else {
// assert(false);
// }
//}
lines.erase(lines.begin(), it);
}
else if (lines[0] == (leadingTabs + "PrincipalMassMarker")) {
lines.erase(lines.begin());
principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
principalMassMarker->parseASMT(lines);
principalMassMarker->owner = this;
}
else if (lines[0] == (leadingTabs + "RefPoints")) {
lines.erase(lines.begin());
refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefCurves"));
std::vector<std::string> refPointsLines(lines.begin(), it);
while (!refPointsLines.empty()) {
if (refPointsLines[0] == (leadingTabs + "\tRefPoint")) {
refPointsLines.erase(refPointsLines.begin());
auto refPoint = CREATE<ASMTRefPoint>::With();
refPoint->parseASMT(refPointsLines);
refPoints->push_back(refPoint);
refPoint->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
}
else if (lines[0] == (leadingTabs + "RefCurves")) {
lines.erase(lines.begin());
refCurves = std::make_shared<std::vector<std::shared_ptr<ASMTRefCurve>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs + "RefSurfaces"));
std::vector<std::string> refCurvesLines(lines.begin(), it);
while (!refCurvesLines.empty()) {
if (refCurvesLines[0] == (leadingTabs + "\tRefCurve")) {
refCurvesLines.erase(refCurvesLines.begin());
auto refCurve = CREATE<ASMTRefCurve>::With();
refCurve->parseASMT(refCurvesLines);
refCurves->push_back(refCurve);
refCurve->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
}
else if (lines[0] == (leadingTabs + "RefSurfaces")) {
lines.erase(lines.begin());
refSurfaces = std::make_shared<std::vector<std::shared_ptr<ASMTRefSurface>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs.substr(0, leadingTabs.size() - 1) + "Part"));
std::vector<std::string> refSurfacesLines(lines.begin(), it);
while (!refSurfacesLines.empty()) {
if (refSurfacesLines[0] == (leadingTabs + "\tRefSurface")) {
refSurfacesLines.erase(refSurfacesLines.begin());
auto refSurface = CREATE<ASMTRefSurface>::With();
refSurface->parseASMT(refSurfacesLines);
refSurfaces->push_back(refSurface);
refSurface->owner = this;
}
else {
assert(false);
}
}
lines.erase(lines.begin(), it);
}
else {
return;
}
}
readName(lines);
readPosition3D(lines);
readRotationMatrix(lines);
readVelocity3D(lines);
readOmega3D(lines);
readFeatureOrder(lines);
readPrincipalMassMarker(lines);
readRefPoints(lines);
readRefCurves(lines);
readRefSurfaces(lines);
}
void MbD::ASMTPart::readFeatureOrder(std::vector<std::string>& lines)
{
assert(lines[0].find("FeatureOrder") != std::string::npos);
lines.erase(lines.begin());
//featureOrder = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
return s.find("PrincipalMassMarker") != std::string::npos;
});
//std::vector<std::string> featureOrderLines(lines.begin(), it);
//while (!featureOrderLines.empty()) {
// if (featureOrderLines[0] == (leadingTabs + "\tExtrusion")) {
// featureOrderLines.erase(featureOrderLines.begin());
// auto extrusion = CREATE<ASMTExtrusion>::With();
// extrusion->parseASMT(featureOrderLines);
// featureOrder->push_back(extrusion);
// extrusion->owner = this;
// }
// else {
// assert(false);
// }
//}
lines.erase(lines.begin(), it);
}
void MbD::ASMTPart::readPrincipalMassMarker(std::vector<std::string>& lines)
{
assert(lines[0].find("PrincipalMassMarker") != std::string::npos);
lines.erase(lines.begin());
principalMassMarker = CREATE<ASMTPrincipalMassMarker>::With();
principalMassMarker->parseASMT(lines);
principalMassMarker->owner = this;
}
void MbD::ASMTPart::readPartSeries(std::vector<std::string>& lines)
{
std::string str = lines[0];
std::string substr = "PartSeries";
auto pos = str.find(substr);
assert(pos != std::string::npos);
str.erase(0, pos + substr.length());
auto seriesName = readString(str);
assert(fullName("") == seriesName);
lines.erase(lines.begin());
//xs, ys, zs, bryxs, bryys, bryzs
readXs(lines);
readYs(lines);
readZs(lines);
readBryantxs(lines);
readBryantys(lines);
readBryantzs(lines);
readVXs(lines);
readVYs(lines);
readVZs(lines);
readOmegaXs(lines);
readOmegaYs(lines);
readOmegaZs(lines);
readAXs(lines);
readAYs(lines);
readAZs(lines);
readAlphaXs(lines);
readAlphaYs(lines);
readAlphaZs(lines);
}
FColDsptr MbD::ASMTPart::vOcmO()
{
auto& rOPO = position3D;
auto& vOPO = velocity3D;
auto& omeOPO = omega3D;
auto rPcmO = rOcmO()->minusFullColumn(rOPO);
return vOPO->plusFullColumn(omeOPO->cross(rPcmO));
}
FColDsptr MbD::ASMTPart::omeOpO()
{
return omega3D;
}

26
MbDCode/ASMTPart.h
View File

@@ -1,26 +1,34 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
#include "ASMTSpatialContainer.h"
#include "ASMTRefPoint.h"
#include "ASMTRefCurve.h"
#include "ASMTRefSurface.h"
#include "ASMTPrincipalMassMarker.h"
#include "PosVelAccData.h"
namespace MbD {
class ASMTPart : public ASMTItem
class ASMTPart : public ASMTSpatialContainer
{
//
public:
void parseASMT(std::vector<std::string>& lines) override;
void readFeatureOrder(std::vector<std::string>& lines);
void readPrincipalMassMarker(std::vector<std::string>& lines);
void readPartSeries(std::vector<std::string>& lines);
FColDsptr vOcmO() override;
FColDsptr omeOpO() override;
std::string name;
FColDsptr position3D, velocity3D, omega3D;
FMatDsptr rotationMatrix;
std::shared_ptr<ASMTPrincipalMassMarker> principalMassMarker;
//std::shared_ptr<std::vector<std::shared_ptr<ASMTFeature>>> featureOrder;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefSurface>>> refSurfaces;
std::shared_ptr<std::vector<std::shared_ptr<PosVelAccData>>> partSeries;
};
}

40
MbDCode/ASMTPointInPlaneJoint.cpp Normal file
View File

@@ -0,0 +1,40 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTPointInPlaneJoint.h"
#include "PointInPlaneJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTPointInPlaneJoint::mbdClassNew()
{
return CREATE<PointInPlaneJoint>::With();
}
void MbD::ASMTPointInPlaneJoint::parseASMT(std::vector<std::string>& lines)
{
ASMTJoint::parseASMT(lines);
readOffset(lines);
}
void MbD::ASMTPointInPlaneJoint::readOffset(std::vector<std::string>& lines)
{
assert(lines[0].find("offset") != std::string::npos);
lines.erase(lines.begin());
offset = readDouble(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTPointInPlaneJoint::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTJoint::createMbD(mbdSys, mbdUnits);
auto pointInPlaneJoint = std::static_pointer_cast<PointInPlaneJoint>(mbdObject);
pointInPlaneJoint->offset = offset;
}

26
MbDCode/ASMTPointInPlaneJoint.h Normal file
View File

@@ -0,0 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTJoint.h"
namespace MbD {
class ASMTPointInPlaneJoint : public ASMTJoint
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
void parseASMT(std::vector<std::string>& lines) override;
void readOffset(std::vector<std::string>& lines);
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
double offset;
};
}

8
MbDCode/ASMTPrincipalMassMarker.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTPrincipalMassMarker.h"
#include <cassert>
#include "FullMatrix.h"

9
MbDCode/ASMTPrincipalMassMarker.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
@@ -9,7 +17,6 @@ namespace MbD {
public:
void parseASMT(std::vector<std::string>& lines) override;
std::string name;
FColDsptr position3D;
FMatDsptr rotationMatrix;
double mass, density;

8
MbDCode/ASMTRefCurve.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTRefCurve.h"
#include "CREATE.h"

12
MbDCode/ASMTRefCurve.h
View File

@@ -1,9 +1,17 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
#include "ASMTRefItem.h"
namespace MbD {
class ASMTRefCurve : public ASMTItem
class ASMTRefCurve : public ASMTRefItem
{
//
public:

37
MbDCode/ASMTRefItem.cpp Normal file
View File

@@ -0,0 +1,37 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTRefItem.h"
#include "CREATE.h"
using namespace MbD;
void MbD::ASMTRefItem::readMarkers(std::vector<std::string>& lines)
{
assert(lines[0].find("Markers") != std::string::npos);
lines.erase(lines.begin());
markers = std::make_shared<std::vector<std::shared_ptr<ASMTMarker>>>();
auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
return s.find("RefPoint") != std::string::npos;
});
std::vector<std::string> markersLines(lines.begin(), it);
while (!markersLines.empty()) {
readMarker(markersLines);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTRefItem::readMarker(std::vector<std::string>& lines)
{
assert(lines[0].find("Marker") != std::string::npos);
lines.erase(lines.begin());
auto marker = CREATE<ASMTMarker>::With();
marker->parseASMT(lines);
markers->push_back(marker);
marker->owner = this;
}

28
MbDCode/ASMTRefItem.h Normal file
View File

@@ -0,0 +1,28 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTSpatialItem.h"
#include "ASMTMarker.h"
namespace MbD {
class ASMTRefItem : public ASMTSpatialItem
{
//
public:
void readMarkers(std::vector<std::string>& lines);
void readMarker(std::vector<std::string>& lines);
std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markers;
};
}

72
MbDCode/ASMTRefPoint.cpp
View File

@@ -1,58 +1,32 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTRefPoint.h"
#include "ASMTMarker.h"
#include "CREATE.h"
using namespace MbD;
void MbD::ASMTRefPoint::parseASMT(std::vector<std::string>& lines)
{
size_t pos = lines[0].find_first_not_of("\t");
auto leadingTabs = lines[0].substr(0, pos);
while (!lines.empty()) {
if (lines[0] == (leadingTabs + "Position3D")) {
lines.erase(lines.begin());
std::istringstream iss(lines[0]);
position3D = std::make_shared<FullColumn<double>>();
double d;
while (iss >> d) {
position3D->push_back(d);
}
lines.erase(lines.begin());
}
else if (lines[0] == (leadingTabs + "RotationMatrix")) {
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
for (int i = 0; i < 3; i++)
{
auto& row = rotationMatrix->at(i);
std::istringstream iss(lines[0]);
double d;
while (iss >> d) {
row->push_back(d);
}
lines.erase(lines.begin());
}
}
else if (lines[0] == (leadingTabs + "Markers")) {
lines.erase(lines.begin());
markers = std::make_shared<std::vector<std::shared_ptr<ASMTMarker>>>();
auto it = std::find(lines.begin(), lines.end(), (leadingTabs.substr(0, leadingTabs.size() - 1) + "RefPoint"));
std::vector<std::string> markersLines(lines.begin(), it);
while (!markersLines.empty()) {
if (markersLines[0] == (leadingTabs + "\tMarker")) {
markersLines.erase(markersLines.begin());
auto marker = CREATE<ASMTMarker>::With();
marker->parseASMT(markersLines);
markers->push_back(marker);
marker->owner = this;
}
else {
return;
}
}
lines.erase(lines.begin(), it);
}
else {
return;
}
readPosition3D(lines);
readRotationMatrix(lines);
readMarkers(lines);
}
std::string MbD::ASMTRefPoint::fullName(std::string partialName)
{
return owner->fullName(partialName);
}
void MbD::ASMTRefPoint::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
for (auto& marker : *markers) {
marker->createMbD(mbdSys, mbdUnits);
}
}

19
MbDCode/ASMTRefPoint.h
View File

@@ -1,20 +1,27 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
#include "ASMTRefItem.h"
#include <vector>
#include <string>
#include "ASMTMarker.h"
namespace MbD {
class ASMTRefPoint : public ASMTItem
class ASMTRefPoint : public ASMTRefItem
{
//
public:
void parseASMT(std::vector<std::string>& lines) override;
std::string fullName(std::string partialName) override;
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
FColDsptr position3D;
FMatDsptr rotationMatrix;
std::shared_ptr<std::vector<std::shared_ptr<ASMTMarker>>> markers;
};
}

8
MbDCode/ASMTRefSurface.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTRefSurface.h"
#include "CREATE.h"

12
MbDCode/ASMTRefSurface.h
View File

@@ -1,9 +1,17 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
#include "ASMTRefItem.h"
namespace MbD {
class ASMTRefSurface : public ASMTItem
class ASMTRefSurface : public ASMTRefItem
{
//
public:

13
MbDCode/ASMTRevoluteJoint.cpp
View File

@@ -1,3 +1,16 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTRevoluteJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTRevoluteJoint::mbdClassNew()
{
return CREATE<RevoluteJoint>::With();
}

11
MbDCode/ASMTRevoluteJoint.h
View File

@@ -1,13 +1,22 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTJoint.h"
#include "RevoluteJoint.h"
namespace MbD {
class ASMTRevoluteJoint : public ASMTJoint
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
};
}

69
MbDCode/ASMTRotationalMotion.cpp
View File

@@ -1,21 +1,66 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTRotationalMotion.h"
#include "ASMTAssembly.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "CREATE.h"
#include "Constant.h"
using namespace MbD;
void MbD::ASMTRotationalMotion::parseASMT(std::vector<std::string>& lines)
{
size_t pos = lines[0].find_first_not_of("\t");
auto leadingTabs = lines[0].substr(0, pos);
assert(lines[0] == (leadingTabs + "Name"));
readName(lines);
readMotionJoint(lines);
readRotationZ(lines);
}
void MbD::ASMTRotationalMotion::readMotionJoint(std::vector<std::string>& lines)
{
assert(lines[0].find("MotionJoint") != std::string::npos);
lines.erase(lines.begin());
name = lines[0];
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "MotionJoint"));
lines.erase(lines.begin());
motionJoint = lines[0];
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "RotationZ"));
lines.erase(lines.begin());
rotationZ = lines[0];
motionJoint = readString(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTRotationalMotion::readRotationZ(std::vector<std::string>& lines)
{
assert(lines[0].find("RotationZ") != std::string::npos);
lines.erase(lines.begin());
rotationZ = readString(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTRotationalMotion::initMarkers()
{
auto jt = root()->jointAt(motionJoint);
markerI = jt->markerI;
markerJ = jt->markerJ;
}
void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTMotion::createMbD(mbdSys, mbdUnits);
auto parser = CREATE<SymbolicParser>::With();
parser->owner = this;
auto geoTime = owner->root()->geoTime();
parser->variables->insert(std::make_pair("time", geoTime));
auto userFunc = CREATE<BasicUserFunction>::With(rotationZ, 1.0);
parser->parseUserFunction(userFunc);
auto geoPhi = parser->stack->top();
geoPhi = Symbolic::times(geoPhi, std::make_shared<Constant>(1.0 / mbdUnits->angle));
geoPhi->createMbD(mbdSys, mbdUnits);
std::static_pointer_cast<ZRotation>(mbdObject)->phiBlk = geoPhi->simplified(geoPhi);
}
std::shared_ptr<Joint> MbD::ASMTRotationalMotion::mbdClassNew()
{
return CREATE<ZRotation>::With();
}

16
MbDCode/ASMTRotationalMotion.h
View File

@@ -1,6 +1,15 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTMotion.h"
#include "ZRotation.h"
namespace MbD {
class ASMTRotationalMotion : public ASMTMotion
@@ -8,8 +17,13 @@ namespace MbD {
//
public:
void parseASMT(std::vector<std::string>& lines) override;
void readMotionJoint(std::vector<std::string>& lines);
void readRotationZ(std::vector<std::string>& lines);
void initMarkers() override;
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
std::shared_ptr<Joint> mbdClassNew() override;
std::string name, motionJoint, rotationZ;
std::string motionJoint, rotationZ;
};
}

8
MbDCode/ASMTSimulationParameters.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTSimulationParameters.h"
using namespace MbD;

26
MbDCode/ASMTSimulationParameters.h
View File

@@ -1,16 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
namespace MbD {
class ASMTSimulationParameters : public ASMTItem
{
//
public:
void parseASMT(std::vector<std::string>& lines) override;
class ASMTSimulationParameters : public ASMTItem
{
//
public:
void parseASMT(std::vector<std::string>& lines) override;
double tstart, tend, hmin, hmax, hout, errorTol;
};
double tstart = 0.0, tend = 1.0, hmin = 1.0e-9, hmax = 1.0e9, hout = 0.1, errorTol = 1.0e-6;
double errorTolPosKine = 1.0e-6, errorTolAccKine = 1.0e-6, corAbsTol = 1.0e-6, corRelTol = 1.0e-6;
double intAbsTol = 1.0e-6, intRelTol = 1.0e-6, translationLimit = 1.0e9, rotationLimit = 1.0e9;
int iterMaxPosKine = 25, iterMaxAccKine = 25, iterMaxDyn = 4, orderMax = 5;
};
}

395
MbDCode/ASMTSpatialContainer.cpp Normal file
View File

@@ -0,0 +1,395 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTSpatialContainer.h"
#include "Units.h"
#include "Part.h"
#include "System.h"
using namespace MbD;
void MbD::ASMTSpatialContainer::initialize()
{
xs = std::make_shared<FullRow<double>>();
ys = std::make_shared<FullRow<double>>();
zs = std::make_shared<FullRow<double>>();
bryxs = std::make_shared<FullRow<double>>();
bryys = std::make_shared<FullRow<double>>();
bryzs = std::make_shared<FullRow<double>>();
vxs = std::make_shared<FullRow<double>>();
vys = std::make_shared<FullRow<double>>();
vzs = std::make_shared<FullRow<double>>();
omexs = std::make_shared<FullRow<double>>();
omeys = std::make_shared<FullRow<double>>();
omezs = std::make_shared<FullRow<double>>();
axs = std::make_shared<FullRow<double>>();
ays = std::make_shared<FullRow<double>>();
azs = std::make_shared<FullRow<double>>();
alpxs = std::make_shared<FullRow<double>>();
alpys = std::make_shared<FullRow<double>>();
alpzs = std::make_shared<FullRow<double>>();
}
void MbD::ASMTSpatialContainer::readRefPoints(std::vector<std::string>& lines)
{
assert(lines[0].find("RefPoints") != std::string::npos);
lines.erase(lines.begin());
refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
return s.find("RefCurves") != std::string::npos;
});
std::vector<std::string> refPointsLines(lines.begin(), it);
while (!refPointsLines.empty()) {
readRefPoint(refPointsLines);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTSpatialContainer::readRefPoint(std::vector<std::string>& lines)
{
assert(lines[0].find("RefPoint") != std::string::npos);
lines.erase(lines.begin());
auto refPoint = CREATE<ASMTRefPoint>::With();
refPoint->parseASMT(lines);
refPoints->push_back(refPoint);
refPoint->owner = this;
}
void MbD::ASMTSpatialContainer::readRefCurves(std::vector<std::string>& lines)
{
assert(lines[0].find("RefCurves") != std::string::npos);
lines.erase(lines.begin());
refCurves = std::make_shared<std::vector<std::shared_ptr<ASMTRefCurve>>>();
auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
return s.find("RefSurfaces") != std::string::npos;
});
std::vector<std::string> refCurvesLines(lines.begin(), it);
while (!refCurvesLines.empty()) {
readRefCurve(refCurvesLines);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTSpatialContainer::readRefCurve(std::vector<std::string>& lines)
{
assert(false);
}
void MbD::ASMTSpatialContainer::readRefSurfaces(std::vector<std::string>& lines)
{
assert(lines[0].find("RefSurfaces") != std::string::npos);
lines.erase(lines.begin());
refSurfaces = std::make_shared<std::vector<std::shared_ptr<ASMTRefSurface>>>();
auto it = std::find_if(lines.begin(), lines.end(), [](const std::string& s) {
return s.find("Part") != std::string::npos;
});
std::vector<std::string> refSurfacesLines(lines.begin(), it);
while (!refSurfacesLines.empty()) {
readRefSurface(refSurfacesLines);
}
lines.erase(lines.begin(), it);
}
void MbD::ASMTSpatialContainer::readRefSurface(std::vector<std::string>& lines)
{
assert(false);
}
void MbD::ASMTSpatialContainer::readXs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "X", inxs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readYs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "Y", inys);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readZs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "Z", inzs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readBryantxs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "Bryantx", inbryxs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readBryantys(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "Bryanty", inbryys);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readBryantzs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "Bryantz", inbryzs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readVXs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "VX", invxs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readVYs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "VY", invys);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readVZs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "VZ", invzs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readOmegaXs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "OmegaX", inomexs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readOmegaYs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "OmegaY", inomeys);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readOmegaZs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "OmegaZ", inomezs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readAXs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "AX", inaxs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readAYs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "AY", inays);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readAZs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "AZ", inazs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readAlphaXs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "AlphaX", inalpxs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readAlphaYs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "AlphaY", inalpys);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::readAlphaZs(std::vector<std::string>& lines)
{
std::string str = lines[0];
readDoublesInto(str, "AlphaZ", inalpzs);
lines.erase(lines.begin());
}
void MbD::ASMTSpatialContainer::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mbdPart = CREATE<Part>::With();
mbdObject = mbdPart;
mbdPart->name = fullName("");
mbdPart->m = principalMassMarker->mass / mbdUnits->mass;
mbdPart->aJ = principalMassMarker->momentOfInertias->times(1.0 / mbdUnits->aJ);
mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length));
mbdPart->qE(qEp());
mbdPart->qXdot(vOcmO()->times(1.0 / mbdUnits->velocity));
mbdPart->omeOpO(omeOpO()->times(1.0 / mbdUnits->omega));
mbdPart->qXddot(std::make_shared<FullColumn<double>>(3, 0));
mbdPart->qEddot(std::make_shared<FullColumn<double>>(4, 0));
mbdSys->addPart(mbdPart);
for (auto& refPoint : *refPoints) {
refPoint->createMbD(mbdSys, mbdUnits);
}
for (auto& refCurve : *refCurves) {
refCurve->createMbD(mbdSys, mbdUnits);
}
for (auto& refSurface : *refSurfaces) {
refSurface->createMbD(mbdSys, mbdUnits);
}
}
FColDsptr MbD::ASMTSpatialContainer::rOcmO()
{
auto& rOPO = position3D;
auto& aAOP = rotationMatrix;
auto& rPcmP = principalMassMarker->position3D;
auto rOcmO = rOPO->plusFullColumn(aAOP->timesFullColumn(rPcmP));
return rOcmO;
}
std::shared_ptr<EulerParameters<double>> MbD::ASMTSpatialContainer::qEp()
{
auto& aAOP = rotationMatrix;
auto& aAPcm = principalMassMarker->rotationMatrix;
auto aAOcm = aAOP->timesFullMatrix(aAPcm);
return aAOcm->asEulerParameters();
}
FColDsptr MbD::ASMTSpatialContainer::vOcmO()
{
assert(false);
return FColDsptr();
}
FColDsptr MbD::ASMTSpatialContainer::omeOpO()
{
assert(false);
return FColDsptr();
}
std::shared_ptr<ASMTSpatialContainer> MbD::ASMTSpatialContainer::part()
{
return std::make_shared<ASMTSpatialContainer>(*this);
}
void MbD::ASMTSpatialContainer::updateFromMbD()
{
auto mbdUnts = mbdUnits();
auto mbdPart = std::static_pointer_cast<Part>(mbdObject);
auto rOcmO = mbdPart->qX()->times(mbdUnts->length);
auto aAOp = mbdPart->aAOp();
auto vOcmO = mbdPart->qXdot()->times(mbdUnts->velocity);
auto omeOPO = mbdPart->omeOpO()->times(mbdUnts->omega);
auto aOcmO = mbdPart->qXddot()->times(mbdUnts->acceleration);
auto alpOPO = mbdPart->alpOpO()->times(mbdUnts->alpha);
auto& rPcmP = principalMassMarker->position3D;
auto& aAPp = principalMassMarker->rotationMatrix;
auto aAOP = aAOp->timesTransposeFullMatrix(aAPp);
auto rPcmO = aAOP->timesFullColumn(rPcmP);
auto rOPO = rOcmO->minusFullColumn(rPcmO);
auto vOPO = vOcmO->minusFullColumn(omeOPO->cross(rPcmO));
auto aOPO = aOcmO->minusFullColumn(alpOPO->cross(rPcmO))->minusFullColumn(omeOPO->cross(omeOPO->cross(rPcmO)));
xs->push_back(rOPO->at(0));
ys->push_back(rOPO->at(1));
zs->push_back(rOPO->at(2));
auto bryantAngles = aAOP->bryantAngles();
bryxs->push_back(bryantAngles->at(0));
bryys->push_back(bryantAngles->at(1));
bryzs->push_back(bryantAngles->at(2));
vxs->push_back(vOPO->at(0));
vys->push_back(vOPO->at(1));
vzs->push_back(vOPO->at(2));
omexs->push_back(omeOPO->at(0));
omeys->push_back(omeOPO->at(1));
omezs->push_back(omeOPO->at(2));
axs->push_back(aOPO->at(0));
ays->push_back(aOPO->at(1));
azs->push_back(aOPO->at(2));
alpxs->push_back(alpOPO->at(0));
alpys->push_back(alpOPO->at(1));
alpzs->push_back(alpOPO->at(2));
}
void MbD::ASMTSpatialContainer::compareResults(AnalysisType type)
{
auto mbdUnts = mbdUnits();
auto factor = 1.0e-6;
auto lengthTol = mbdUnts->length * factor;
auto angleTol = mbdUnts->angle * factor;
auto velocityTol = mbdUnts->velocity * factor;
auto omegaTol = mbdUnts->omega * factor;
auto accelerationTol = mbdUnts->acceleration * factor;
auto alphaTol = mbdUnts->alpha * factor;
auto i = xs->size() - 1;
//Pos
if (!Numeric::equaltol(xs->at(i), inxs->at(i), lengthTol)) {
std::cout << i << " xs " << xs->at(i) << ", " << i<< lengthTol << std::endl;
std::cout << i << " xs " << xs->at(i) << ", " << inxs->at(i) << ", " << lengthTol << std::endl;
}
if (!Numeric::equaltol(ys->at(i), inys->at(i), lengthTol)) {
std::cout << i << " ys " << ys->at(i) << ", " << inys->at(i) << ", " << lengthTol << std::endl;
}
if (!Numeric::equaltol(zs->at(i), inzs->at(i), lengthTol)) {
std::cout << i << " zs " << zs->at(i) << ", " << inzs->at(i) << ", " << lengthTol << std::endl;
}
if (!Numeric::equaltol(bryxs->at(i), inbryxs->at(i), angleTol)) {
std::cout << i << " bryxs " << bryxs->at(i) << ", " << inbryxs->at(i) << ", " << angleTol << std::endl;
}
if (!Numeric::equaltol(bryys->at(i), inbryys->at(i), angleTol)) {
std::cout << i << " bryys " << bryys->at(i) << ", " << inbryys->at(i) << ", " << angleTol << std::endl;
}
if (!Numeric::equaltol(bryzs->at(i), inbryzs->at(i), angleTol)) {
std::cout << i << " bryzs " << bryzs->at(i) << ", " << inbryzs->at(i) << ", " << angleTol << std::endl;
}
//Vel
if (!Numeric::equaltol(vxs->at(i), invxs->at(i), velocityTol)) {
std::cout << i << " vxs " << vxs->at(i) << ", " << invxs->at(i) << ", " << velocityTol << std::endl;
}
if (!Numeric::equaltol(vys->at(i), invys->at(i), velocityTol)) {
std::cout << i << " vys " << vys->at(i) << ", " << invys->at(i) << ", " << velocityTol << std::endl;
}
if (!Numeric::equaltol(vzs->at(i), invzs->at(i), velocityTol)) {
std::cout << i << " vzs " << vzs->at(i) << ", " << invzs->at(i) << ", " << velocityTol << std::endl;
}
if (!Numeric::equaltol(omexs->at(i), inomexs->at(i), omegaTol)) {
std::cout << i << " omexs " << omexs->at(i) << ", " << inomexs->at(i) << ", " << omegaTol << std::endl;
}
if (!Numeric::equaltol(omeys->at(i), inomeys->at(i), omegaTol)) {
std::cout << i << " omeys " << omeys->at(i) << ", " << inomeys->at(i) << ", " << omegaTol << std::endl;
}
if (!Numeric::equaltol(omezs->at(i), inomezs->at(i), omegaTol)) {
std::cout << i << " omezs " << omezs->at(i) << ", " << inomezs->at(i) << ", " << omegaTol << std::endl;
}
//Acc
if (!Numeric::equaltol(axs->at(i), inaxs->at(i), accelerationTol)) {
std::cout << i << " axs " << axs->at(i) << ", " << inaxs->at(i) << ", " << accelerationTol << std::endl;
}
if (!Numeric::equaltol(ays->at(i), inays->at(i), accelerationTol)) {
std::cout << i << " ays " << ays->at(i) << ", " << inays->at(i) << ", " << accelerationTol << std::endl;
}
if (!Numeric::equaltol(azs->at(i), inazs->at(i), accelerationTol)) {
std::cout << i << " azs " << azs->at(i) << ", " << inazs->at(i) << ", " << accelerationTol << std::endl;
}
if (!Numeric::equaltol(alpxs->at(i), inalpxs->at(i), alphaTol)) {
std::cout << i << " alpxs " << alpxs->at(i) << ", " << inalpxs->at(i) << ", " << alphaTol << std::endl;
}
if (!Numeric::equaltol(alpys->at(i), inalpys->at(i), alphaTol)) {
std::cout << i << " alpys " << alpys->at(i) << ", " << inalpys->at(i) << ", " << alphaTol << std::endl;
}
if (!Numeric::equaltol(alpzs->at(i), inalpzs->at(i), alphaTol)) {
std::cout << i << " alpzs " << alpzs->at(i) << ", " << inalpzs->at(i) << ", " << alphaTol << std::endl;
}
}

70
MbDCode/ASMTSpatialContainer.h Normal file
View File

@@ -0,0 +1,70 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTSpatialItem.h"
#include "ASMTRefPoint.h"
#include "ASMTRefCurve.h"
#include "ASMTRefSurface.h"
#include "ASMTPrincipalMassMarker.h"
#include "Units.h"
namespace MbD {
class ASMTSpatialContainer : public ASMTSpatialItem
{
//
public:
void initialize() override;
void readRefPoints(std::vector<std::string>& lines);
void readRefPoint(std::vector<std::string>& lines);
void readRefCurves(std::vector<std::string>& lines);
void readRefCurve(std::vector<std::string>& lines);
void readRefSurfaces(std::vector<std::string>& lines);
void readRefSurface(std::vector<std::string>& lines);
void readXs(std::vector<std::string>& lines);
void readYs(std::vector<std::string>& lines);
void readZs(std::vector<std::string>& lines);
void readBryantxs(std::vector<std::string>& lines);
void readBryantys(std::vector<std::string>& lines);
void readBryantzs(std::vector<std::string>& lines);
void readVXs(std::vector<std::string>& lines);
void readVYs(std::vector<std::string>& lines);
void readVZs(std::vector<std::string>& lines);
void readOmegaXs(std::vector<std::string>& lines);
void readOmegaYs(std::vector<std::string>& lines);
void readOmegaZs(std::vector<std::string>& lines);
void readAXs(std::vector<std::string>& lines);
void readAYs(std::vector<std::string>& lines);
void readAZs(std::vector<std::string>& lines);
void readAlphaXs(std::vector<std::string>& lines);
void readAlphaYs(std::vector<std::string>& lines);
void readAlphaZs(std::vector<std::string>& lines);
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
FColDsptr rOcmO();
std::shared_ptr<EulerParameters<double>> qEp();
virtual FColDsptr vOcmO();
virtual FColDsptr omeOpO();
std::shared_ptr<ASMTSpatialContainer> part() override;
void updateFromMbD() override;
void compareResults(AnalysisType type) override;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefPoint>>> refPoints;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefCurve>>> refCurves;
std::shared_ptr<std::vector<std::shared_ptr<ASMTRefSurface>>> refSurfaces;
FRowDsptr xs, ys, zs, bryxs, bryys, bryzs;
FRowDsptr vxs, vys, vzs, omexs, omeys, omezs;
FRowDsptr axs, ays, azs, alpxs, alpys, alpzs;
FRowDsptr inxs, inys, inzs, inbryxs, inbryys, inbryzs;
FRowDsptr invxs, invys, invzs, inomexs, inomeys, inomezs;
FRowDsptr inaxs, inays, inazs, inalpxs, inalpys, inalpzs;
std::shared_ptr<ASMTPrincipalMassMarker> principalMassMarker;
};
}

67
MbDCode/ASMTSpatialItem.cpp Normal file
View File

@@ -0,0 +1,67 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTSpatialItem.h"
using namespace MbD;
void MbD::ASMTSpatialItem::readPosition3D(std::vector<std::string>& lines)
{
assert(lines[0].find("Position3D") != std::string::npos);
lines.erase(lines.begin());
std::istringstream iss(lines[0]);
position3D = std::make_shared<FullColumn<double>>();
double d;
while (iss >> d) {
position3D->push_back(d);
}
lines.erase(lines.begin());
}
void MbD::ASMTSpatialItem::readRotationMatrix(std::vector<std::string>& lines)
{
assert(lines[0].find("RotationMatrix") != std::string::npos);
lines.erase(lines.begin());
rotationMatrix = std::make_shared<FullMatrix<double>>(3, 0);
for (int i = 0; i < 3; i++)
{
auto& row = rotationMatrix->at(i);
std::istringstream iss(lines[0]);
double d;
while (iss >> d) {
row->push_back(d);
}
lines.erase(lines.begin());
}
}
void MbD::ASMTSpatialItem::readVelocity3D(std::vector<std::string>& lines)
{
assert(lines[0].find("Velocity3D") != std::string::npos);
lines.erase(lines.begin());
std::istringstream iss(lines[0]);
velocity3D = std::make_shared<FullColumn<double>>();
double d;
while (iss >> d) {
velocity3D->push_back(d);
}
lines.erase(lines.begin());
}
void MbD::ASMTSpatialItem::readOmega3D(std::vector<std::string>& lines)
{
assert(lines[0].find("Omega3D") != std::string::npos);
lines.erase(lines.begin());
std::istringstream iss(lines[0]);
omega3D = std::make_shared<FullColumn<double>>();
double d;
while (iss >> d) {
omega3D->push_back(d);
}
lines.erase(lines.begin());
}

28
MbDCode/ASMTSpatialItem.h Normal file
View File

@@ -0,0 +1,28 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTItem.h"
namespace MbD {
class ASMTSpatialItem : public ASMTItem
{
//
public:
void readPosition3D(std::vector<std::string>& lines);
void readRotationMatrix(std::vector<std::string>& lines);
void readVelocity3D(std::vector<std::string>& lines);
void readOmega3D(std::vector<std::string>& lines);
FColDsptr position3D, velocity3D, omega3D;
FMatDsptr rotationMatrix;
};
}

16
MbDCode/ASMTSphericalJoint.cpp Normal file
View File

@@ -0,0 +1,16 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTSphericalJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTSphericalJoint::mbdClassNew()
{
return CREATE<SphericalJoint>::With();
}

23
MbDCode/ASMTSphericalJoint.h Normal file
View File

@@ -0,0 +1,23 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTJoint.h"
#include "SphericalJoint.h"
namespace MbD {
class ASMTSphericalJoint : public ASMTJoint
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
};
}

31
MbDCode/ASMTTime.cpp Normal file
View File

@@ -0,0 +1,31 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include <cassert>
#include "ASMTTime.h"
#include "Time.h"
#include "Constant.h"
#include "Product.h"
using namespace MbD;
void MbD::ASMTTime::deleteMbD()
{
xx = nullptr;
expression = nullptr;
}
void MbD::ASMTTime::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
auto mbdTime = mbdSys->time;
if (xx == mbdTime) return;
auto timeScale = std::make_shared<Constant>(mbdUnits->time);
auto geoTime = std::make_shared<Product>(timeScale, mbdTime);
this->xexpression(mbdTime, geoTime->simplified(geoTime));
}

25
MbDCode/ASMTTime.h Normal file
View File

@@ -0,0 +1,25 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ExpressionX.h"
#include "System.h"
#include "Units.h"
namespace MbD {
class ASMTTime : public ExpressionX
{
//
public:
void deleteMbD();
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits);
};
}

16
MbDCode/ASMTTranslationalJoint.cpp Normal file
View File

@@ -0,0 +1,16 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTTranslationalJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTTranslationalJoint::mbdClassNew()
{
return CREATE<TranslationalJoint>::With();
}

24
MbDCode/ASMTTranslationalJoint.h Normal file
View File

@@ -0,0 +1,24 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTJoint.h"
#include "TranslationalJoint.h"
namespace MbD {
class ASMTTranslationalJoint : public ASMTJoint
{
//
public:
std::shared_ptr<Joint> mbdClassNew() override;
};
}

83
MbDCode/ASMTTranslationalMotion.cpp
View File

@@ -1,21 +1,80 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTTranslationalMotion.h"
#include "ASMTAssembly.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "Constant.h"
using namespace MbD;
void MbD::ASMTTranslationalMotion::parseASMT(std::vector<std::string>& lines)
{
size_t pos = lines[0].find_first_not_of("\t");
auto leadingTabs = lines[0].substr(0, pos);
assert(lines[0] == (leadingTabs + "Name"));
readName(lines);
readMotionJoint(lines);
readTranslationZ(lines);
//size_t pos = lines[0].find_first_not_of("\t");
//auto leadingTabs = lines[0].substr(0, pos);
//assert(lines[0] == (leadingTabs + "Name"));
//lines.erase(lines.begin());
//name = lines[0];
//lines.erase(lines.begin());
//assert(lines[0] == (leadingTabs + "MotionJoint"));
//lines.erase(lines.begin());
//motionJoint = lines[0];
//lines.erase(lines.begin());
//assert(lines[0] == (leadingTabs + "TranslationZ"));
//lines.erase(lines.begin());
//translationZ = lines[0];
//lines.erase(lines.begin());
}
void MbD::ASMTTranslationalMotion::initMarkers()
{
auto jt = root()->jointAt(motionJoint);
markerI = jt->markerI;
markerJ = jt->markerJ;
}
void MbD::ASMTTranslationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits)
{
ASMTMotion::createMbD(mbdSys, mbdUnits);
auto parser = CREATE<SymbolicParser>::With();
parser->owner = this;
auto geoTime = owner->root()->geoTime();
parser->variables->insert(std::make_pair("time", geoTime));
auto userFunc = CREATE<BasicUserFunction>::With(translationZ, 1.0);
parser->parseUserFunction(userFunc);
auto zIJ = parser->stack->top();
zIJ = Symbolic::times(zIJ, std::make_shared<Constant>(1.0 / mbdUnits->length));
zIJ->createMbD(mbdSys, mbdUnits);
std::static_pointer_cast<ZTranslation>(mbdObject)->zBlk = zIJ->simplified(zIJ);
}
std::shared_ptr<Joint> MbD::ASMTTranslationalMotion::mbdClassNew()
{
return CREATE<ZTranslation>::With();
}
void MbD::ASMTTranslationalMotion::readMotionJoint(std::vector<std::string>& lines)
{
assert(lines[0].find("MotionJoint") != std::string::npos);
lines.erase(lines.begin());
name = lines[0];
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "MotionJoint"));
lines.erase(lines.begin());
motionJoint = lines[0];
lines.erase(lines.begin());
assert(lines[0] == (leadingTabs + "TranslationZ"));
lines.erase(lines.begin());
translationZ = lines[0];
motionJoint = readString(lines[0]);
lines.erase(lines.begin());
}
void MbD::ASMTTranslationalMotion::readTranslationZ(std::vector<std::string>& lines)
{
assert(lines[0].find("TranslationZ") != std::string::npos);
lines.erase(lines.begin());
translationZ = readString(lines[0]);
lines.erase(lines.begin());
}

16
MbDCode/ASMTTranslationalMotion.h
View File

@@ -1,6 +1,15 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTMotion.h"
#include "ZTranslation.h"
namespace MbD {
class ASMTTranslationalMotion : public ASMTMotion
@@ -8,8 +17,13 @@ namespace MbD {
//
public:
void parseASMT(std::vector<std::string>& lines) override;
void initMarkers() override;
void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
std::shared_ptr<Joint> mbdClassNew() override;
void readMotionJoint(std::vector<std::string>& lines);
void readTranslationZ(std::vector<std::string>& lines);
std::string name, motionJoint, translationZ;
std::string motionJoint, translationZ;
};
}

17
MbDCode/ASMTUniversalJoint.cpp Normal file
View File

@@ -0,0 +1,17 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ASMTUniversalJoint.h"
#include "UniversalJoint.h"
using namespace MbD;
std::shared_ptr<Joint> MbD::ASMTUniversalJoint::mbdClassNew()
{
return CREATE<UniversalJoint>::With();
}

22
MbDCode/ASMTUniversalJoint.h Normal file
View File

@@ -0,0 +1,22 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ASMTJoint.h"
namespace MbD {
class ASMTUniversalJoint : public ASMTJoint
{
//
public:
virtual std::shared_ptr<Joint> mbdClassNew() override;
};
}

26
MbDCode/Abs.cpp Normal file
View File

@@ -0,0 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "Abs.h"
using namespace MbD;
MbD::Abs::Abs(Symsptr arg) : FunctionX(arg)
{
}
double MbD::Abs::getValue()
{
return std::abs(xx->getValue());
}
std::ostream& MbD::Abs::printOn(std::ostream& s) const
{
s << "abs(" << xx << ")";
return s;
}

26
MbDCode/Abs.h Normal file
View File

@@ -0,0 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "FunctionX.h"
namespace MbD {
class Abs : public FunctionX
{
//
public:
Abs() = default;
Abs(Symsptr arg);
double getValue() override;
std::ostream& printOn(std::ostream& s) const override;
};
}

16
MbDCode/AbsConstraint.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AbsConstraint.h"
#include "PartFrame.h"
@@ -15,10 +23,10 @@ AbsConstraint::AbsConstraint(int i)
void AbsConstraint::calcPostDynCorrectorIteration()
{
if (axis < 3) {
aG = static_cast<PartFrame*>(owner)->qX->at(axis);
aG = static_cast<PartFrame*>(owner)->qX->at((size_t)axis);
}
else {
aG = static_cast<PartFrame*>(owner)->qE->at(axis - 3);
aG = static_cast<PartFrame*>(owner)->qE->at((size_t)axis - 3);
}
}
@@ -55,10 +63,10 @@ void AbsConstraint::fillAccICIterError(FColDsptr col)
auto partFrame = static_cast<PartFrame*>(owner);
double sum;
if (axis < 3) {
sum = partFrame->qXddot->at(axis);
sum = partFrame->qXddot->at((size_t)axis);
}
else {
sum = partFrame->qEddot->at(axis - 3);
sum = partFrame->qEddot->at((size_t)axis - 3);
}
col->atiplusNumber(iG, sum);
}

8
MbDCode/AbsConstraint.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "Constraint.h"

8
MbDCode/AccICKineNewtonRaphson.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AccICKineNewtonRaphson.h"
#include "SystemSolver.h"

14
MbDCode/AccICKineNewtonRaphson.h
View File

@@ -1,11 +1,19 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "AccNewtonRaphson.h"
#include "AccICNewtonRaphson.h"
namespace MbD {
class AccICKineNewtonRaphson : public AccNewtonRaphson
class AccICKineNewtonRaphson : public AccICNewtonRaphson
{
//
//Kinematics with under constrained system
public:
void initializeGlobally() override;
void preRun() override;

10
MbDCode/AccICNewtonRaphson.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AccICNewtonRaphson.h"
#include "SystemSolver.h"
@@ -10,7 +18,7 @@ bool AccICNewtonRaphson::isConverged()
void AccICNewtonRaphson::preRun()
{
std::string str("MbD: Solving for quasi kinematic acceleration.");
std::string str("MbD: Solving for acceleration initial conditions.");
system->logString(str);
AccNewtonRaphson::preRun();
}

10
MbDCode/AccICNewtonRaphson.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "AccNewtonRaphson.h"
@@ -5,7 +13,7 @@
namespace MbD {
class AccICNewtonRaphson : public AccNewtonRaphson
{
//
//IC acceleration with fully or under constrained system
public:
bool isConverged() override;
void preRun() override;

8
MbDCode/AccKineNewtonRaphson.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AccKineNewtonRaphson.h"
#include "SystemSolver.h"

10
MbDCode/AccKineNewtonRaphson.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "AccNewtonRaphson.h"
@@ -5,7 +13,7 @@
namespace MbD {
class AccKineNewtonRaphson : public AccNewtonRaphson
{
//
//Kinematics with fully constrained system
public:
void initializeGlobally() override;
void preRun() override;

10
MbDCode/AccNewtonRaphson.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AccNewtonRaphson.h"
#include "SystemSolver.h"
#include "Part.h"
@@ -58,7 +66,7 @@ void AccNewtonRaphson::fillY()
item->fillAccICIterError(y);
//std::cout << item->name << *y << std::endl;
});
std::cout << *y << std::endl;
//std::cout << *y << std::endl;
}
void AccNewtonRaphson::incrementIterNo()

8
MbDCode/AccNewtonRaphson.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "SystemNewtonRaphson.h"

8
MbDCode/AngleJoint.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AngleJoint.h"
#include "CREATE.h"
#include "System.h"

8
MbDCode/AngleJoint.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "Joint.h"

10
MbDCode/AngleZIecJec.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include <corecrt_math_defines.h>
#include "AngleZIecJec.h"
@@ -9,7 +17,7 @@ MbD::AngleZIecJec::AngleZIecJec()
{
}
MbD::AngleZIecJec::AngleZIecJec(EndFrmcptr frmi, EndFrmcptr frmj) : KinematicIeJe(frmi, frmj)
MbD::AngleZIecJec::AngleZIecJec(EndFrmsptr frmi, EndFrmsptr frmj) : KinematicIeJe(frmi, frmj)
{
}

10
MbDCode/AngleZIecJec.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "KinematicIeJe.h"
@@ -9,7 +17,7 @@ namespace MbD {
//thez aA00IeJe aA10IeJe cosOverSSq sinOverSSq twoCosSinOverSSqSq dSqOverSSqSq
public:
AngleZIecJec();
AngleZIecJec(EndFrmcptr frmi, EndFrmcptr frmj);
AngleZIecJec(EndFrmsptr frmi, EndFrmsptr frmj);
void calcPostDynCorrectorIteration() override;
virtual void init_aAijIeJe() = 0;

10
MbDCode/AngleZIeqcJec.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AngleZIeqcJec.h"
#include "CREATE.h"
@@ -7,7 +15,7 @@ MbD::AngleZIeqcJec::AngleZIeqcJec()
{
}
MbD::AngleZIeqcJec::AngleZIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj) : AngleZIecJec(frmi, frmj)
MbD::AngleZIeqcJec::AngleZIeqcJec(EndFrmsptr frmi, EndFrmsptr frmj) : AngleZIecJec(frmi, frmj)
{
}

10
MbDCode/AngleZIeqcJec.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "AngleZIecJec.h"
@@ -8,7 +16,7 @@ namespace MbD {
//pthezpEI ppthezpEIpEI pcthezpEI psthezpEI
public:
AngleZIeqcJec();
AngleZIeqcJec(EndFrmcptr frmi, EndFrmcptr frmj);
AngleZIeqcJec(EndFrmsptr frmi, EndFrmsptr frmj);
void calcPostDynCorrectorIteration() override;
void init_aAijIeJe() override;

10
MbDCode/AngleZIeqcJeqc.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AngleZIeqcJeqc.h"
#include "CREATE.h"
#include "DirectionCosineIeqcJeqc.h"
@@ -8,7 +16,7 @@ MbD::AngleZIeqcJeqc::AngleZIeqcJeqc()
{
}
MbD::AngleZIeqcJeqc::AngleZIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj) : AngleZIeqcJec(frmi, frmj)
MbD::AngleZIeqcJeqc::AngleZIeqcJeqc(EndFrmsptr frmi, EndFrmsptr frmj) : AngleZIeqcJec(frmi, frmj)
{
}

10
MbDCode/AngleZIeqcJeqc.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "AngleZIeqcJec.h"
@@ -8,7 +16,7 @@ namespace MbD {
//pthezpEJ ppthezpEIpEJ ppthezpEJpEJ
public:
AngleZIeqcJeqc();
AngleZIeqcJeqc(EndFrmcptr frmi, EndFrmcptr frmj);
AngleZIeqcJeqc(EndFrmsptr frmi, EndFrmsptr frmj);
void calcPostDynCorrectorIteration() override;
void init_aAijIeJe() override;

15
MbDCode/AnyGeneralSpline.cpp Normal file
View File

@@ -0,0 +1,15 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AnyGeneralSpline.h"
using namespace MbD;
MbD::AnyGeneralSpline::AnyGeneralSpline(Symsptr arg) : FunctionFromData(arg)
{
}

21
MbDCode/AnyGeneralSpline.h Normal file
View File

@@ -0,0 +1,21 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "FunctionFromData.h"
namespace MbD {
class AnyGeneralSpline : public FunctionFromData
{
//derivs degree index delta
public:
AnyGeneralSpline() = default;
AnyGeneralSpline(Symsptr arg);
};
}

8
MbDCode/AnyPosICNewtonRaphson.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AnyPosICNewtonRaphson.h"
#include "SystemSolver.h"
#include "Item.h"

13
MbDCode/AnyPosICNewtonRaphson.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "PosNewtonRaphson.h"
@@ -7,6 +15,7 @@ namespace MbD {
class AnyPosICNewtonRaphson : public PosNewtonRaphson
{
//IC with fully or under constrained system
//nqsu qsuOld qsuWeights nSingularMatrixError
public:
void initialize() override;
@@ -18,8 +27,8 @@ namespace MbD {
void assignEquationNumbers() = 0;
int nqsu = -1;
std::shared_ptr<FullColumn<double>> qsuOld;
std::shared_ptr<DiagonalMatrix<double>> qsuWeights;
FColDsptr qsuOld;
DiagMatDsptr qsuWeights;
int nSingularMatrixError = -1;
};
}

26
MbDCode/ArcTan.cpp Normal file
View File

@@ -0,0 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ArcTan.h"
using namespace MbD;
MbD::ArcTan::ArcTan(Symsptr arg) : FunctionX(arg)
{
}
double MbD::ArcTan::getValue()
{
return std::atan(xx->getValue());
}
std::ostream& MbD::ArcTan::printOn(std::ostream& s) const
{
s << "arctan(" << xx << ")";
return s;
}

26
MbDCode/ArcTan.h Normal file
View File

@@ -0,0 +1,26 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "FunctionX.h"
namespace MbD {
class ArcTan : public FunctionX
{
//
public:
ArcTan() = default;
ArcTan(Symsptr arg);
double getValue() override;
std::ostream& printOn(std::ostream& s) const override;
};
}

20
MbDCode/ArcTan2.cpp Normal file
View File

@@ -0,0 +1,20 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "ArcTan2.h"
using namespace MbD;
MbD::ArcTan2::ArcTan2(Symsptr arg, Symsptr arg1) : FunctionXY(arg, arg1)
{
}
double MbD::ArcTan2::getValue()
{
return std::atan2(y->getValue(), x->getValue());;
}

25
MbDCode/ArcTan2.h Normal file
View File

@@ -0,0 +1,25 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "FunctionXY.h"
namespace MbD {
class ArcTan2 : public FunctionXY
{
//
public:
ArcTan2() = default;
ArcTan2(Symsptr arg, Symsptr arg1);
double getValue() override;
};
}

8
MbDCode/Array.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "Array.h"
using namespace MbD;

100
MbDCode/Array.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include <ostream>
@@ -6,8 +14,13 @@
#include <cmath>
#include <cassert>
//#include "Symbolic.h"
namespace MbD {
using ListD = std::initializer_list<double>;
using ListListD = std::initializer_list<std::initializer_list<double>>;
using ListListPairD = std::initializer_list<std::initializer_list<std::initializer_list<double>>>;
template<typename T>
class Array : public std::vector<T>
{
@@ -25,9 +38,16 @@ namespace MbD {
double rootMeanSquare();
virtual int numberOfElements();
void swapElems(int i, int ii);
//double maxMagnitude();
virtual double maxMagnitude() = 0;
double maxMagnitudeOfVector();
void equalArrayAt(std::shared_ptr<Array<T>> array, int i);
//virtual void normalizeSelf();
//virtual void conditionSelf();
//virtual void conditionSelfWithTol(double tol);
virtual void atiput(int i, T value);
//double length();
void magnifySelf(T factor);
void atitimes(int i, double factor);
virtual std::ostream& printOn(std::ostream& s) const {
std::string str = typeid(*this).name();
@@ -76,21 +96,17 @@ namespace MbD {
this->at(i) = this->at(ii);
this->at(ii) = temp;
}
//template<typename T>
//inline double Array<T>::maxMagnitude()
//template<>
//inline double Array<double>::maxMagnitude()
//{
// if (std::is_arithmetic<T>::value) {
// return this->maxMagnitudeOfVector();
// }
// else {
// auto answer = 0.0;
// for (int i = 0; i < this->size(); i++)
// {
// auto mag = this->at(i)->maxMagnitude();
// if (answer < mag) answer = mag;
// }
// return answer;
// auto max = 0.0;
// for (int i = 0; i < this->size(); i++)
// {
// auto element = this->at(i);
// if (element < 0.0) element = -element;
// if (max < element) max = element;
// }
// return max;
//}
template<typename T>
inline double Array<T>::maxMagnitudeOfVector()
@@ -111,7 +127,57 @@ namespace MbD {
this->at(ii) = array->at(i + ii);
}
}
using ListD = std::initializer_list<double>;
using ListListD = std::initializer_list<std::initializer_list<double>>;
using ListListPairD = std::initializer_list<std::initializer_list<std::initializer_list<double>>>;
//template<>
//inline void Array<double>::normalizeSelf()
//{
// auto length = this->length();
// if (length == 0.0) throw std::runtime_error("Cannot normalize a null vector.");
// this->magnifySelf(1.0 / length);
//}
//template<>
//inline void Array<double>::conditionSelf()
//{
// constexpr auto epsilon = std::numeric_limits<double>::epsilon();
// auto tol = maxMagnitude() * epsilon;
// conditionSelfWithTol(tol);
//}
//template<>
//inline void Array<double>::conditionSelfWithTol(double tol)
//{
// for (int i = 0; i < this->size(); i++)
// {
// auto element = this->at(i);
// if (element < 0.0) element = -element;
// if (element < tol) this->atiput(i, 0.0);
// }
//}
template<typename T>
inline void Array<T>::atiput(int i, T value)
{
this->at(i) = value;
}
//template<>
//inline double Array<double>::length()
//{
// auto ssq = 0.0;
// for (int i = 0; i < this->size(); i++)
// {
// auto elem = this->at(i);
// ssq += elem * elem;
// }
// return std::sqrt(ssq);
//}
template<typename T>
inline void Array<T>::magnifySelf(T factor)
{
for (int i = 0; i < this->size(); i++)
{
this->atitimes(i, factor);
}
}
template<typename T>
inline void Array<T>::atitimes(int i, double factor)
{
this->at(i) *= factor;
}
}

10
MbDCode/AtPointConstraintIJ.cpp
View File

@@ -1,10 +1,18 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AtPointConstraintIJ.h"
#include "DispCompIecJecO.h"
#include "CREATE.h"
using namespace MbD;
AtPointConstraintIJ::AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
AtPointConstraintIJ::AtPointConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj, int axisi) :
ConstraintIJ(frmi, frmj), axis(axisi)
{
}

10
MbDCode/AtPointConstraintIJ.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "ConstraintIJ.h"
@@ -9,7 +17,7 @@ namespace MbD {
{
//axis riIeJeO
public:
AtPointConstraintIJ(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
AtPointConstraintIJ(EndFrmsptr frmi, EndFrmsptr frmj, int axisi);
void calcPostDynCorrectorIteration() override;
void initialize() override;

10
MbDCode/AtPointConstraintIqcJc.cpp
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#include "AtPointConstraintIqcJc.h"
#include "DispCompIeqcJecO.h"
#include "CREATE.h"
@@ -5,7 +13,7 @@
using namespace MbD;
AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi) :
AtPointConstraintIqcJc::AtPointConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj, int axisi) :
AtPointConstraintIJ(frmi, frmj, axisi)
{
}

10
MbDCode/AtPointConstraintIqcJc.h
View File

@@ -1,3 +1,11 @@
/***************************************************************************
* Copyright (c) 2023 Ondsel, Inc. *
* *
* This file is part of OndselSolver. *
* *
* See LICENSE file for details about copyright. *
***************************************************************************/
#pragma once
#include "AtPointConstraintIJ.h"
@@ -7,7 +15,7 @@ namespace MbD {
{
//pGpEI ppGpEIpEI iqXIminusOnePlusAxis iqEI
public:
AtPointConstraintIqcJc(EndFrmcptr frmi, EndFrmcptr frmj, int axisi);
AtPointConstraintIqcJc(EndFrmsptr frmi, EndFrmsptr frmj, int axisi);
void addToJointForceI(FColDsptr col) override;
void addToJointTorqueI(FColDsptr col) override;

Some files were not shown because too many files have changed in this diff Show More