void WCModeSketchArcThreePointCreate::OnMouseMove(const WPFloat &x, const WPFloat &y) {
//Get suggestion from workbench
this->_xSuggest = this->_workbench->SnapMouseX();
this->_ySuggest = this->_workbench->SnapMouseY();
this->_suggestionType = this->_workbench->SuggestAlignment(this->_alignRules, NULL, this->_xSuggest, this->_ySuggest);
//Init some variables
WCVector4 newPoint(this->_xSuggest, this->_ySuggest, 0.0, 1.0);
//Get the updated position
newPoint = this->_workbench->Sketch()->ReferencePlane()->TransformMatrix() * newPoint;
//Switch on click stage
if (this->_stage == 1) {
//Update second point
this->_secondPoint->Set(newPoint);
//Update end of firstLine
this->_firstLine->End(newPoint);
}
//Second click has occured
else if (this->_stage == 2) {
//Update third point
this->_thirdPoint->Set(newPoint);
//Calculate center and radius
WCVector4 center;
WPFloat radius = WCSketchArc::CalculateCenterAndRadius(this->_firstPoint->Data(), this->_secondPoint->Data(), this->_thirdPoint->Data(), center);
if (radius == 0.0) {
//Clean up and wait for good radius
if (this->_arc != NULL) delete this->_arc;
this->_arc = NULL;
//Make first line visible
this->_firstLine->IsVisible(true);
}
else {
//Make first line not visible
this->_firstLine->IsVisible(false);
//Recreate arc
WCVector4 xUnit(1.0, 0.0, 0.0, 0.0);
WCVector4 yUnit(0.0, 1.0, 0.0, 0.0);
xUnit = this->_workbench->Sketch()->ReferencePlane()->TransformMatrix() * xUnit;
yUnit = this->_workbench->Sketch()->ReferencePlane()->TransformMatrix() * yUnit;
xUnit.Normalize(true);
yUnit.Normalize(true);
//Back project center onto the plane to determine rotation angles
WCVector4 planeCenter = this->_workbench->Sketch()->ReferencePlane()->InverseTransformMatrix() * center;
WPFloat firstAngle = WCSketchArc::PointsToDegrees(planeCenter, this->_first);
WPFloat secondAngle = WCSketchArc::PointsToDegrees(planeCenter, this->_second);
//If not on right, then swap first and second
if (!IsOnRight2D(this->_first.I(), this->_first.J(), this->_second.I(), this->_second.J(), this->_workbench->SnapMouseX(), this->_workbench->SnapMouseY()))
std::swap(firstAngle, secondAngle);
//Delete the old arc
if (this->_arc != NULL) delete this->_arc;
//Create the new one
this->_arc = WCNurbsCurve::CircularArc(this->_creator->Document()->Scene()->GeometryContext(),
center, xUnit, yUnit, radius, firstAngle, secondAngle);
this->_arc->Color(WCSketchFeature::InprocessColor);
this->_arc->Thickness(WCSketchFeature::LineThickness);
// this->_arc->Layer( this->_workbench->Layer() );
}
}
}
void Render(WCSketchWorkbench *wb, const WPFloat &x, const WPFloat &y) {
WPFloat iconSize = 16 * SCREEN_PIXEL_WIDTH;
WPFloat localX = x + 0.01;
WPFloat localY = y + 0.01;
//Calculate box slighly above and right of location
WCVector4 ll(localX, localY, 0.0, 1.0);
WCVector4 xUnit(1.0, 0.0, 0.0, 1.0);
WCVector4 yUnit(0.0, 1.0, 0.0, 1.0);
//Project from the plane (adjust for zoom factor to maintain icon size)
ll = wb->Sketch()->ReferencePlane()->TransformMatrix() * ll;
xUnit = wb->Sketch()->ReferencePlane()->TransformMatrix() * xUnit;
yUnit = wb->Sketch()->ReferencePlane()->TransformMatrix() * yUnit;
xUnit = xUnit * (iconSize / wb->Feature()->Document()->Scene()->ActiveCamera()->Zoom());
yUnit = yUnit * (iconSize / wb->Feature()->Document()->Scene()->ActiveCamera()->Zoom());
WCVector4 ul = ll + yUnit;
WCVector4 ur = ll + xUnit + yUnit;
WCVector4 lr = ll + xUnit;
//Set up data arrays
GLfloat data[12] = {(GLfloat)ll.I(), (GLfloat)ll.J(), (GLfloat)ll.K(),
(GLfloat)ul.I(), (GLfloat)ul.J(), (GLfloat)ul.K(),
(GLfloat)ur.I(), (GLfloat)ur.J(), (GLfloat)ur.K(),
(GLfloat)lr.I(), (GLfloat)lr.J(), (GLfloat)lr.K()};
WSTexture *tex = wb->Feature()->Document()->Scene()->TextureManager()->TextureFromName("concentric32");
GLfloat texCoords[8] = { 0.0, 0.0,
0.0, tex->_height,
tex->_width, tex->_height,
tex->_width, 0.0 };
//Enable state
glDisable(GL_DEPTH_TEST);
glEnable(tex->_target);
glEnableClientState(GL_VERTEX_ARRAY);
glBindTexture(tex->_target, tex->_id);
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
glColor4f(1.0, 1.0, 1.0, 1.0);
//Draw textured quad
glVertexPointer(3, GL_FLOAT, 0, data);
glTexCoordPointer(2, GL_FLOAT, 0, texCoords);
glDrawArrays(GL_QUADS, 0, 4);
//Clean up state
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
glBindTexture(tex->_target, 0);
glDisableClientState(GL_VERTEX_ARRAY);
glDisable(tex->_target);
glEnable(GL_DEPTH_TEST);
}
void WCModeSketchArcThreePointCreate::OnMouseDown(const WCMouseButton &button) {
//Restart if right click
if (button == WCMouseButton::Right()) {
if(this->_stage == 0) return;
//Delete first point, second point, and first line
delete this->_firstPoint;
delete this->_secondPoint;
delete this->_firstLine;
//See if in stage 2
if (this->_stage > 1) {
//Delete third point and arc
delete this->_thirdPoint;
if (this->_arc != NULL) {
delete this->_arc;
this->_arc = NULL;
}
}
//Reset the stage
this->_stage = 0;
return;
}
else if (button != WCMouseButton::Left()) return;
//Init some variables
WCVector4 newPoint(this->_workbench->SnapMouseX(), this->_workbench->SnapMouseY(), 0.0, 1.0);
newPoint = this->_workbench->Sketch()->ReferencePlane()->TransformMatrix() * newPoint;
//Switch on first click
if (this->_stage == 0) {
//Get the position
this->_first.I( this->_workbench->SnapMouseX() );
this->_first.J( this->_workbench->SnapMouseY() );
//Create first point
this->_firstPoint = new WCGeometricPoint(newPoint);
//Create second point and first line
this->_secondPoint = new WCGeometricPoint(newPoint);
this->_firstLine = new WCGeometricLine(newPoint, newPoint);
//Set the properties of the points and lines
this->_firstPoint->Color(WCSketchFeature::InprocessColor);
this->_firstPoint->Size(WCSketchFeature::PointSize);
this->_secondPoint->Color(WCSketchFeature::InprocessColor);
this->_secondPoint->Size(WCSketchFeature::PointSize);
this->_firstLine->Color(WCSketchFeature::InprocessColor);
this->_firstLine->Thickness(WCSketchFeature::LineThickness);
//Increment stage
this->_stage++;
return;
}
//Second click
else if (this->_stage == 1) {
//Record second click
this->_second.I( this->_workbench->SnapMouseX() );
this->_second.J( this->_workbench->SnapMouseY() );
//Create the third point and second line
this->_thirdPoint = new WCGeometricPoint(newPoint);
//Increment the stage
this->_stage++;
return;
}
//Last click
else {
//Calculate center and radius
WCVector4 center;
WPFloat radius = WCSketchArc::CalculateCenterAndRadius(this->_firstPoint->Data(), this->_secondPoint->Data(), newPoint, center);
if (radius == 0.0) {
//Clean up and wait for good radius
if (this->_arc != NULL) delete this->_arc;
this->_arc = NULL;
}
//All is good
else {
//Recreate arc
WCVector4 xUnit(1.0, 0.0, 0.0, 0.0);
WCVector4 yUnit(0.0, 1.0, 0.0, 0.0);
xUnit = this->_workbench->Sketch()->ReferencePlane()->TransformMatrix() * xUnit;
yUnit = this->_workbench->Sketch()->ReferencePlane()->TransformMatrix() * yUnit;
xUnit.Normalize(true);
yUnit.Normalize(true);
//Back project center onto the plane to determine rotation angles
WCVector4 planeCenter = this->_workbench->Sketch()->ReferencePlane()->InverseTransformMatrix() * center;
WPFloat firstAngle = WCSketchArc::PointsToDegrees(planeCenter, this->_first);
WPFloat secondAngle = WCSketchArc::PointsToDegrees(planeCenter, this->_second);
//If not on right, swap angles
if (!IsOnRight2D(this->_first.I(), this->_first.J(), this->_second.I(), this->_second.J(), this->_workbench->SnapMouseX(), this->_workbench->SnapMouseY()))
std::swap(firstAngle, secondAngle);
//Determine center planar coordinates
center = this->_workbench->Sketch()->ReferencePlane()->InverseTransformMatrix() * center;
//Create the arc creation action
WCAction *action = WCSketchArc::ActionCreate(this->_workbench->Sketch(), "", center, radius, firstAngle, secondAngle,
WCVector4(this->_workbench->MouseX(), this->_workbench->MouseY(), 0.0, 1.0));
//Execute the action
this->_workbench->Sketch()->Document()->ExecuteAction( action );
//Clean up the arc
delete this->_arc;
this->_arc = NULL;
}
//Delete the drawing objects
delete this->_firstPoint;
delete this->_secondPoint;
delete this->_firstLine;
delete this->_thirdPoint;
//Reset stage
this->_stage = 0;
}
}
//---------------------------------------------------------------------------
void ObjectReferencedAxes::CalculateRotationMatrix(const A1Mjd &atEpoch,
bool forceComputation)
{
if (!primary)
throw CoordinateSystemException("Primary \"" + primaryName +
"\" is not yet set in object referenced coordinate system!");
if (!secondary)
throw CoordinateSystemException("Secondary \"" + secondaryName +
"\" is not yet set in object referenced coordinate system!");
if ((xAxis == yAxis) || (xAxis == zAxis) || (yAxis == zAxis))
{
CoordinateSystemException cse;
cse.SetDetails("For object referenced axes, axes are improperly "
"defined.\nXAxis = '%s', YAxis = '%s', ZAxis = '%s'",
xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
throw cse;
}
if ((xAxis != "") && (yAxis != "") && (zAxis != ""))
{
CoordinateSystemException cse;
cse.SetDetails("For object referenced axes, too many axes are defined.\n"
"XAxis = '%s', YAxis = '%s', ZAxis = '%s'",
xAxis.c_str(), yAxis.c_str(), zAxis.c_str());
throw cse;
}
SpacePoint *useAsSecondary = secondary;
// if (!useAsSecondary) useAsSecondary = origin;
Rvector6 rv = useAsSecondary->GetMJ2000State(atEpoch) -
primary->GetMJ2000State(atEpoch);
#ifdef DEBUG_ROT_MATRIX
if (visitCount == 0)
{
MessageInterface::ShowMessage(" ------------ rv Primary (%s) to Secondary (%s) = %s\n",
primary->GetName().c_str(), secondary->GetName().c_str(), rv.ToString().c_str());
visitCount++;
}
#endif
#ifdef DEBUG_ROT_MATRIX
if (visitCount == 0)
{
std::stringstream ss;
ss.precision(30);
ss << " ----------------- rv Earth to Moon (truncated) = "
<< rv << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
visitCount++;
}
#endif
Rvector3 a = useAsSecondary->GetMJ2000Acceleration(atEpoch) -
primary->GetMJ2000Acceleration(atEpoch);
Rvector3 r = rv.GetR();
Rvector3 v = rv.GetV();
Rvector3 n = Cross(r,v);
Rvector3 rUnit = r.GetUnitVector();
Rvector3 vUnit = v.GetUnitVector();
Rvector3 nUnit = n.GetUnitVector();
Real rMag = r.GetMagnitude();
Real vMag = v.GetMagnitude();
Real nMag = n.GetMagnitude();
// check for divide-by-zero
if ((GmatMathUtil::IsEqual(rMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(vMag, MAGNITUDE_TOL)) || (GmatMathUtil::IsEqual(nMag, MAGNITUDE_TOL)))
{
std::string errmsg = "Object referenced axis system named \"";
errmsg += coordName + "\" is undefined because at least one axis is near zero in length.\n";
throw CoordinateSystemException(errmsg);
}
Rvector3 rDot = (v / rMag) - (rUnit / rMag) * (rUnit * v);
Rvector3 vDot = (a / vMag) - (vUnit / vMag) * (vUnit * a);
Rvector3 nDot = (Cross(r,a) / nMag) - (nUnit / nMag) * (Cross(r,a) * nUnit);
Rvector3 xUnit, yUnit, zUnit, xDot, yDot, zDot;
bool xUsed = true, yUsed = true, zUsed = true;
// determine the x-axis
if ((xAxis == "R") || (xAxis == "r"))
{
xUnit = rUnit;
xDot = rDot;
}
else if ((xAxis == "-R") || (xAxis == "-r"))
{
xUnit = -rUnit;
xDot = -rDot;
}
else if ((xAxis == "V") || (xAxis == "v"))
{
xUnit = vUnit;
xDot = vDot;
}
else if ((xAxis == "-V") || (xAxis == "-v"))
{
xUnit = -vUnit;
xDot = -vDot;
}
else if ((xAxis == "N") || (xAxis == "n"))
{
xUnit = nUnit;
xDot = nDot;
}
else if ((xAxis == "-N") || (xAxis == "-n"))
{
xUnit = -nUnit;
xDot = -nDot;
}
else
{
xUsed = false;
}
// determine the y-axis
if ((yAxis == "R") || (yAxis == "r"))
{
yUnit = rUnit;
yDot = rDot;
}
else if ((yAxis == "-R") || (yAxis == "-r"))
{
yUnit = -rUnit;
yDot = -rDot;
}
else if ((yAxis == "V") || (yAxis == "v"))
{
yUnit = vUnit;
yDot = vDot;
}
else if ((yAxis == "-V") || (yAxis == "-v"))
{
yUnit = -vUnit;
yDot = -vDot;
}
else if ((yAxis == "N") || (yAxis == "n"))
{
yUnit = nUnit;
yDot = nDot;
}
else if ((yAxis == "-N") || (yAxis == "-n"))
{
yUnit = -nUnit;
yDot = -nDot;
}
else
{
yUsed = false;
}
// determine the z-axis
if ((zAxis == "R") || (zAxis == "r"))
{
zUnit = rUnit;
zDot = rDot;
}
else if ((zAxis == "-R") || (zAxis == "-r"))
{
zUnit = -rUnit;
zDot = -rDot;
}
else if ((zAxis == "V") || (zAxis == "v"))
{
zUnit = vUnit;
zDot = vDot;
}
else if ((zAxis == "-V") || (zAxis == "-v"))
{
zUnit = -vUnit;
zDot = -vDot;
}
else if ((zAxis == "N") || (zAxis == "n"))
{
zUnit = nUnit;
zDot = nDot;
}
else if ((zAxis == "-N") || (zAxis == "-n"))
{
zUnit = -nUnit;
zDot = -nDot;
}
else
{
zUsed = false;
}
// determine the third axis
if (xUsed && yUsed && !zUsed)
{
zUnit = Cross(xUnit, yUnit);
zDot = Cross(xDot, yUnit) + Cross(xUnit, yDot);
}
else if (xUsed && zUsed && !yUsed)
{
yUnit = Cross(zUnit,xUnit);
yDot = Cross(zDot, xUnit) + Cross(zUnit, xDot);
}
else if (yUsed && zUsed && !xUsed)
{
xUnit = Cross(yUnit,zUnit);
xDot = Cross(yDot, zUnit) + Cross(yUnit, zDot);
}
else
{
throw CoordinateSystemException(
"Object referenced axes are improperly defined.");
}
// Compute the rotation matrix
rotMatrix(0,0) = xUnit(0);
rotMatrix(0,1) = yUnit(0);
rotMatrix(0,2) = zUnit(0);
rotMatrix(1,0) = xUnit(1);
rotMatrix(1,1) = yUnit(1);
rotMatrix(1,2) = zUnit(1);
rotMatrix(2,0) = xUnit(2);
rotMatrix(2,1) = yUnit(2);
rotMatrix(2,2) = zUnit(2);
// Compute the rotation derivative matrix
rotDotMatrix(0,0) = xDot(0);
rotDotMatrix(0,1) = yDot(0);
rotDotMatrix(0,2) = zDot(0);
rotDotMatrix(1,0) = xDot(1);
rotDotMatrix(1,1) = yDot(1);
rotDotMatrix(1,2) = zDot(1);
rotDotMatrix(2,0) = xDot(2);
rotDotMatrix(2,1) = yDot(2);
rotDotMatrix(2,2) = zDot(2);
#ifdef DEBUG_ROT_MATRIX
MessageInterface::ShowMessage
("rotMatrix=%s\n", rotMatrix.ToString().c_str());
std::stringstream ss;
ss.setf(std::ios::fixed);
ss.precision(30);
ss << " ----------------- rotMatrix = " << rotMatrix << std::endl;
ss.setf(std::ios::scientific);
ss << " ----------------- rotDotMatrix = " << rotDotMatrix << std::endl;
MessageInterface::ShowMessage("%s\n", ss.str().c_str());
#endif
if (!rotMatrix.IsOrthonormal(ORTHONORMAL_TOL))
{
std::stringstream errmsg("");
errmsg << "*** WARNING*** Object referenced axis system \"" << coordName;
errmsg << "\" has a non-orthogonal rotation matrix. " << std::endl;
}
}
/*!
Calcola gli AlgorithmPointI sulla base di points, verificando di punto in punto lo step
tra ascissa e ordinata; inserisce i punti calcolati in gPoints e in base alla quntita' di
AlgorithmPointI trovati restituisce true(e' possibile interpolare) o false(non e' possibile).
\param points Vettore di T3DPoints
*/
bool interpolate(const std::vector<T3DPointD> &points)
{
unsigned int curr, next;
TPointI currStep, xStep, yStep, guideLine;
TPointI xUnit(1, 0);
TPointI yUnit(0, 1);
bool chooseByDistance = false;
curr = 0;
next = 1;
//int i = points.size();
while (next <= points.size() - 1) {
if (points[next] != points[curr]) {
gPoints.push_back(AlgorithmPointI(points[curr], curr));
guideLine = TPointI((int)(points[curr].x - points[next].x),
(int)(points[curr].y - points[next].y));
currStep = gPoints.back();
double xStepTheta, yStepTheta;
//TPointI a;
while (norm2(TPointI((int)(points[next].x - currStep.x), (int)(points[next].y - currStep.y))) > 1) {
TPointI nextPoint = TPointI((int)points[next].x, (int)points[next].y);
//TPointI a = nextPoint - currStep;
//int i = 0;
if (currStep.x > nextPoint.x)
xStep = currStep - xUnit;
else if (currStep.x < nextPoint.x)
xStep = currStep + xUnit;
else {
xStep = currStep;
chooseByDistance = true;
}
if (currStep.y > nextPoint.y)
yStep = currStep - yUnit;
else if (currStep.y < nextPoint.y)
yStep = currStep + yUnit;
else {
yStep = currStep;
chooseByDistance = true;
}
if (chooseByDistance) {
chooseByDistance = false;
if (norm2(xStep - nextPoint) < norm2(yStep - nextPoint))
currStep = xStep;
else
currStep = yStep;
} else {
double aux = norm2(guideLine);
xStepTheta = acos(
(xStep - nextPoint) * guideLine /
(sqrt(norm2(xStep - nextPoint) * aux)));
yStepTheta = acos(
(yStep - nextPoint) * guideLine /
(sqrt(norm2(yStep - nextPoint) * aux)));
if (xStepTheta > yStepTheta)
currStep = yStep;
else
currStep = xStep;
}
gPoints.push_back(AlgorithmPointI(currStep, next));
}
}
curr++;
next++;
}
gPoints.push_back(AlgorithmPointI(points[curr], curr));
if ((int)gPoints.size() < (2 * gMaxDist + 1))
return false;
return true;
}
