void OpenSimContext::setLocation(PathPoint& mp, int i, double d) {
mp.setLocation(*_configState, i, d);
_configState->invalidateAll(SimTK::Stage::Position);
_model->getMultibodySystem().realize(*_configState, SimTK::Stage::Position);
mp.getPath()->updateGeometry(*_configState);
return;
}
/*
* Add a new path point, with default location, to the path.
*
* @param aIndex The position in the pathPointSet to put the new point in.
* @param aBody The body to attach the point to.
* @return Pointer to the newly created path point.
*/
PathPoint* GeometryPath::
addPathPoint(const SimTK::State& s, int aIndex, PhysicalFrame& aBody)
{
PathPoint* newPoint = new PathPoint();
newPoint->setBody(aBody);
Vec3& location = newPoint->getLocation();
placeNewPathPoint(s, location, aIndex, aBody);
upd_PathPointSet().insert(aIndex, newPoint);
// Rename the path points starting at this new one.
namePathPoints(aIndex);
// Update start point and end point in the wrap instances so that they
// refer to the same path points they did before the new point
// was added. These indices are 1-based.
aIndex++;
for (int i=0; i<get_PathWrapSet().getSize(); i++) {
int startPoint = get_PathWrapSet().get(i).getStartPoint();
int endPoint = get_PathWrapSet().get(i).getEndPoint();
if (startPoint != -1 && aIndex <= startPoint)
get_PathWrapSet().get(i).setStartPoint(s,startPoint + 1);
if (endPoint != -1 && aIndex <= endPoint)
get_PathWrapSet().get(i).setEndPoint(s,endPoint + 1);
}
return newPoint;
}
/**
* Calculate the wrapping of one path segment over one wrap object.
*
* @param aPoint1 The first patth point
* @param aPoint2 The second path point
* @param aPathWrap An object holding the parameters for this path/wrap-object pairing
* @param aWrapResult The result of the wrapping (tangent points, etc.)
* @return The status, as a WrapAction enum
*/
int WrapObject::wrapPathSegment(const SimTK::State& s, PathPoint& aPoint1, PathPoint& aPoint2,
const PathWrap& aPathWrap, WrapResult& aWrapResult) const
{
int return_code = noWrap;
bool p_flag;
Vec3 pt1(0.0);
Vec3 pt2(0.0);
// Convert the path points from the frames of the bodies they are attached
// to to the frame of the wrap object's body
_model->getSimbodyEngine().transformPosition(s, aPoint1.getBody(), aPoint1.getLocation(), getBody(), pt1);
_model->getSimbodyEngine().transformPosition(s, aPoint2.getBody(), aPoint2.getLocation(), getBody(), pt2);
// Convert the path points from the frame of the wrap object's body
// into the frame of the wrap object
pt1 = _pose.shiftBaseStationToFrame(pt1);
pt2 = _pose.shiftBaseStationToFrame(pt2);
return_code = wrapLine(s, pt1, pt2, aPathWrap, aWrapResult, p_flag);
if (p_flag == true && return_code > 0) {
// Convert the tangent points from the frame of the wrap object to the
// frame of the wrap object's body
aWrapResult.r1 = _pose.shiftFrameStationToBase(aWrapResult.r1);
aWrapResult.r2 = _pose.shiftFrameStationToBase(aWrapResult.r2);
// Convert the surface points (between the tangent points) from the frame of
// the wrap object to the frame of the wrap object's body
for (int i = 0; i < aWrapResult.wrap_pts.getSize(); i++)
aWrapResult.wrap_pts.get(i) = _pose.shiftFrameStationToBase(aWrapResult.wrap_pts.get(i));
}
return return_code;
}
DBOOL PathListData::Copy(HOBJECT hPathPoint)
{
CServerDE* pServerDE = BaseClass::GetServerDE();
if (!hPathPoint || !pServerDE) return DFALSE;
// Get the Object Position
pServerDE->GetObjectPos(hPathPoint, &m_vPos);
char* pName = pServerDE->GetObjectName(hPathPoint);
if (pName)
{
m_hstrName = pServerDE->CreateString(pName);
}
// Get the Objects action
PathPoint *pPathPoint = (PathPoint*)pServerDE->HandleToObject(hPathPoint);
HSTRING hstrAction = pPathPoint->GetActionTarget();
if (hstrAction)
{
m_hstrActionTarget = pServerDE->CopyString(hstrAction);
}
hstrAction = pPathPoint->GetActionMessage();
if (hstrAction)
{
m_hstrActionMessage = pServerDE->CopyString(hstrAction);
}
return DTRUE;
}
PathPoint* GeometryPath::
appendNewPathPoint(const std::string& proposedName,
PhysicalFrame& aBody, const SimTK::Vec3& aPositionOnBody)
{
PathPoint* newPoint = new PathPoint();
newPoint->setBody(aBody);
newPoint->setName(proposedName);
newPoint->set_location(aPositionOnBody);
upd_PathPointSet().adoptAndAppend(newPoint);
return newPoint;
}
PathPoint PathPoint::withChangedPointType (const Path::Iterator::PathElementType newType,
const Rectangle<int>& parentArea) const
{
PathPoint p (*this);
if (newType != p.type)
{
int oldNumPoints = getNumPoints();
p.type = newType;
int numPoints = p.getNumPoints();
if (numPoints != oldNumPoints)
{
double lastX, lastY;
double x, y, w, h;
p.pos [numPoints - 1] = p.pos [oldNumPoints - 1];
p.pos [numPoints - 1].getRectangleDouble (x, y, w, h, parentArea, owner->getDocument()->getComponentLayout());
const int index = owner->points.indexOf (this);
if (PathPoint* lastPoint = owner->points [index - 1])
{
lastPoint->pos [lastPoint->getNumPoints() - 1]
.getRectangleDouble (lastX, lastY, w, h, parentArea, owner->getDocument()->getComponentLayout());
}
else
{
jassertfalse;
lastX = x;
lastY = y;
}
for (int i = 0; i < numPoints - 1; ++i)
{
p.pos[i] = p.pos [numPoints - 1];
p.pos[i].updateFrom (lastX + (x - lastX) * (i + 1) / numPoints,
lastY + (y - lastY) * (i + 1) / numPoints,
w, h,
parentArea,
owner->getDocument()->getComponentLayout());
}
}
}
return p;
}
PathPoint* PathPoint::makePathPointOfType(PathPoint* aPoint, const string& aNewTypeName)
{
PathPoint* newPoint = NULL;
cout << "PathPoint::makePathPointOfType()" << endl;
if (aPoint != NULL) {
Object* newObject = Object::newInstanceOfType(aNewTypeName);
if (newObject) {
newPoint = dynamic_cast<PathPoint*>(newObject);
if (newPoint) {
// Copy the contents from aPoint.
newPoint->init(*aPoint);
}
}
}
return newPoint;
}
void BezierController::MakeSegment(float time,
float inc,
const PathPoint& p1,
const PathPoint& p2,
const PathPoint& p3,
const PathPoint& p4)
{
osg::Vec3 pos, tangent;
for (int i = 0; i < 3; ++i)
{
pos[i] = (BlendFunction(inc,0) * p1.GetPosition()[i]) +
(BlendFunction(inc,1) * p2.GetPosition()[i]) +
(BlendFunction(inc,2) * p3.GetPosition()[i]) +
(BlendFunction(inc,3) * p4.GetPosition()[i]);
//tangent[i] = (TangentFunction(inc,1) * p2.GetPosition()[i]) + (TangentFunction(inc,2) * p3.GetPosition()[i]) + (TangentFunction(inc,3) * p4.GetPosition()[i]);
}
osg::Quat quat;
quat.slerp(1.0 - inc, p4.GetOrientation(), p1.GetOrientation());
PathData pd;
pd.mTime = time;
pd.mPoint.SetPosition(pos);
pd.mPoint.SetOrientation(quat);
mPath.push_back(pd);
}
// get the path as PointForceDirections directions
// CAUTION: the return points are heap allocated; you must delete them yourself!
// (TODO: that is really lame)
void GeometryPath::
getPointForceDirections(const SimTK::State& s,
OpenSim::Array<PointForceDirection*> *rPFDs) const
{
int i;
PathPoint* start;
PathPoint* end;
const OpenSim::PhysicalFrame* startBody;
const OpenSim::PhysicalFrame* endBody;
const Array<PathPoint*>& currentPath = getCurrentPath(s);
int np = currentPath.getSize();
rPFDs->ensureCapacity(np);
for (i = 0; i < np; i++) {
PointForceDirection *pfd =
new PointForceDirection(currentPath[i]->getLocation(),
currentPath[i]->getBody(), Vec3(0));
rPFDs->append(pfd);
}
for (i = 0; i < np-1; i++) {
start = currentPath[i];
end = currentPath[i+1];
startBody = &start->getBody();
endBody = &end->getBody();
if (startBody != endBody)
{
Vec3 posStart, posEnd;
Vec3 direction(0);
// Find the positions of start and end in the inertial frame.
//engine.getPosition(s, start->getBody(), start->getLocation(), posStart);
posStart = start->getLocationInGround(s);
//engine.getPosition(s, end->getBody(), end->getLocation(), posEnd);
posEnd = end->getLocationInGround(s);
// Form a vector from start to end, in the inertial frame.
direction = (posEnd - posStart);
// Check that the two points are not coincident.
// This can happen due to infeasible wrapping of the path,
// when the origin or insertion enters the wrapping surface.
// This is a temporary fix, since the wrap algorithm should
// return NaN for the points and/or throw an Exception- aseth
if (direction.norm() < SimTK::SignificantReal){
direction = direction*SimTK::NaN;
}
else{
direction = direction.normalize();
}
// Get resultant direction at each point
rPFDs->get(i)->addToDirection(direction);
rPFDs->get(i+1)->addToDirection(-direction);
}
}
}
void PaintElementPath::setCurrentBounds (const Rectangle<int>& b,
const Rectangle<int>& parentArea,
const bool /*undoable*/)
{
Rectangle<int> newBounds (b);
newBounds.setSize (jmax (1, newBounds.getWidth()),
jmax (1, newBounds.getHeight()));
const Rectangle<int> current (getCurrentBounds (parentArea));
if (newBounds != current)
{
const int borderSize = getBorderSize();
const int dx = newBounds.getX() - current.getX();
const int dy = newBounds.getY() - current.getY();
const double scaleStartX = current.getX() + borderSize;
const double scaleStartY = current.getY() + borderSize;
const double scaleX = (newBounds.getWidth() - borderSize * 2) / (double) (current.getWidth() - borderSize * 2);
const double scaleY = (newBounds.getHeight() - borderSize * 2) / (double) (current.getHeight() - borderSize * 2);
for (int i = 0; i < points.size(); ++i)
{
PathPoint* const destPoint = points.getUnchecked(i);
PathPoint p (*destPoint);
for (int j = p.getNumPoints(); --j >= 0;)
rescalePoint (p.pos[j], dx, dy,
scaleX, scaleY,
scaleStartX, scaleStartY,
parentArea);
perform (new ChangePointAction (destPoint, i, p), "Move path");
}
}
}
PathPoint Path::update(Vector3<float> refPos, Vector3<float> playerPos)
{
float D_val;
PathPoint current = getCurrent();
PathPoint previous = getPrevious();
float diffX = refPos.x - current.getPosition().x;
float diffY = refPos.y - current.getPosition().y;
float diffZ = refPos.z - current.getPosition().z;
float playerDistFromPlane = 0;
float firstDistFromPlane = 0;
Vector3<float> vect1 (current.getPosition().x - previous.getPosition().x,
current.getPosition().y - previous.getPosition().y,
current.getPosition().z - previous.getPosition().z);
Vector3<float> vect2 (current.getPosition().x + current.getUp().x,
current.getPosition().y + current.getUp().y,
current.getPosition().z + current.getUp().z);
Vector3<float> normal;
float distance = sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ);
PathPoint firstChoice = getAt(current.getFirstID());
if (distance < RANGE_CHECK) {
if (current.getNumberOfIDs() == 1) {
setChoice(current.getFirstID());
return getCurrent();
}
normal = vect1.Cross(vect2).Normalized();
D_val = (current.getPosition().x * normal.x + current.getPosition().y
* normal.y + current.getPosition().z * normal.z) * -1.0f;
playerDistFromPlane = (normal.x * playerPos.x) +
(normal.y * playerPos.y) + (normal.z * playerPos.z) + D_val;
if (abs(playerDistFromPlane) < MID_BUFFER_WIDTH &&
current.getNumberOfIDs() == 3) {
setChoice(current.getThirdID());
return getCurrent();
}
firstDistFromPlane = normal.x * firstChoice.getPosition().x
+ normal.y * firstChoice.getPosition().y
+ normal.z * firstChoice.getPosition().z + D_val;
if (playerDistFromPlane / abs(playerDistFromPlane) ==
firstDistFromPlane / abs(firstDistFromPlane)) {
setChoice(current.getFirstID());
return getCurrent();
}
else {
setChoice(current.getSecondID());
return getCurrent();
}
}
return getCurrent();
}
bool OpenSimContext::isActivePathPoint(PathPoint& mp) {
return mp.isActive(*_configState);
};
void OpenSimContext::setBody(PathPoint& pathPoint, Body& newBody) {
pathPoint.changeBodyPreserveLocation(*_configState, newBody);
this->recreateSystemAfterSystemExists();
realizeVelocity();
}
/* add in the equivalent spatial forces on bodies for an applied tension
along the GeometryPath to a set of bodyForces */
void GeometryPath::addInEquivalentForces(const SimTK::State& s,
const double& tension,
SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
SimTK::Vector& mobilityForces) const
{
PathPoint* start = NULL;
PathPoint* end = NULL;
const SimTK::MobilizedBody* bo = NULL;
const SimTK::MobilizedBody* bf = NULL;
const Array<PathPoint*>& currentPath = getCurrentPath(s);
int np = currentPath.getSize();
const SimTK::SimbodyMatterSubsystem& matter =
getModel().getMatterSubsystem();
// start point, end point, direction, and force vectors in ground
Vec3 po(0), pf(0), dir(0), force(0);
// partial velocity of point in body expressed in ground
Vec3 dPodq_G(0), dPfdq_G(0);
// gen force (torque) due to moving point under tension
double fo, ff;
for (int i = 0; i < np-1; ++i) {
start = currentPath[i];
end = currentPath[i+1];
bo = &start->getBody().getMobilizedBody();
bf = &end->getBody().getMobilizedBody();
if (bo != bf) {
// Find the positions of start and end in the inertial frame.
po = start->getLocationInGround(s);
pf = end->getLocationInGround(s);
// Form a vector from start to end, in the inertial frame.
dir = (pf - po);
// Check that the two points are not coincident.
// This can happen due to infeasible wrapping of the path,
// when the origin or insertion enters the wrapping surface.
// This is a temporary fix, since the wrap algorithm should
// return NaN for the points and/or throw an Exception- aseth
if (dir.norm() < SimTK::SignificantReal){
dir = dir*SimTK::NaN;
}
else{
dir = dir.normalize();
}
force = tension*dir;
const MovingPathPoint* mppo =
dynamic_cast<MovingPathPoint *>(start);
// do the same for the end point of this segment of the path
const MovingPathPoint* mppf =
dynamic_cast<MovingPathPoint *>(end);
// add in the tension point forces to body forces
if (mppo) {// moving path point location is a function of the state
// transform of the frame of the point to the base mobilized body
auto X_BF = mppo->getParentFrame().findTransformInBaseFrame();
bo->applyForceToBodyPoint(s, X_BF*mppo->getLocation(s), force,
bodyForces);
}
else {
// transform of the frame of the point to the base mobilized body
auto X_BF = start->getParentFrame().findTransformInBaseFrame();
bo->applyForceToBodyPoint(s, X_BF*start->getLocation(), force,
bodyForces);
}
if (mppf) {// moving path point location is a function of the state
// transform of the frame of the point to the base mobilized body
auto X_BF = mppf->getParentFrame().findTransformInBaseFrame();
bf->applyForceToBodyPoint(s, X_BF*mppf->getLocation(s), -force,
bodyForces);
}
else {
// transform of the frame of the point to the base mobilized body
auto X_BF = end->getParentFrame().findTransformInBaseFrame();
bf->applyForceToBodyPoint(s, X_BF*end->getLocation(), -force,
bodyForces);
}
// Now account for the work being done by virtue of the moving
// path point motion relative to the body it is on
if(mppo){
// torque (genforce) contribution due to relative movement
// of a via point w.r.t. the body it is connected to.
dPodq_G = bo->expressVectorInGroundFrame(s, start->getdPointdQ(s));
fo = ~dPodq_G*force;
// get the mobilized body the coordinate is couple to.
const SimTK::MobilizedBody& mpbod =
matter.getMobilizedBody(mppo->getXCoordinate().getBodyIndex());
// apply the generalized (mobility) force to the coordinate's body
mpbod.applyOneMobilityForce(s,
mppo->getXCoordinate().getMobilizerQIndex(),
fo, mobilityForces);
}
if(mppf){
dPfdq_G = bf->expressVectorInGroundFrame(s, end->getdPointdQ(s));
ff = ~dPfdq_G*(-force);
// get the mobilized body the coordinate is couple to.
const SimTK::MobilizedBody& mpbod =
matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex());
mpbod.applyOneMobilityForce(s,
mppf->getXCoordinate().getMobilizerQIndex(),
ff, mobilityForces);
}
}
}
}