Vec3d DistWeightFunc::spacedw(const Vec3d& _q) const { Vec3d result(0.0, 0.0, 0.0); double d = _q.norm(); if (d <= m_t && d != 0.0) // if d==0, then dw is not defined result = (_q / (d * m_t)) * m_wk->df(d / m_t); return result; }
void glutMotion(int x, int y) { if (gIsRotatingCamera) { static const double kTrackBallRadius = 0.8; Vec3d lastPos; lastPos[0] = gLastMouseX * 2.0 / gWindowWidth - 1.0; lastPos[1] = (gWindowHeight - gLastMouseY) * 2.0 / gWindowHeight - 1.0; lastPos[2] = projectToTrackball(kTrackBallRadius, lastPos[0], lastPos[1]); Vec3d currPos; currPos[0] = x * 2.0 / gWindowWidth - 1.0; currPos[1] = (gWindowHeight - y) * 2.0 / gWindowHeight - 1.0; currPos[2] = projectToTrackball(kTrackBallRadius, currPos[0], currPos[1]); currPos.normalize(); lastPos.normalize(); Vec3d rotateVec = lastPos.cross(currPos); double rotateAngle = asin(rotateVec.norm()); if (fabs(rotateAngle) > 1e-6) { double deltaRotation[16]; generateRotationMatrix(deltaRotation, rotateAngle, rotateVec[0], rotateVec[1], rotateVec[2]); multRight(gCameraRotation, deltaRotation); updateCamera(); } } else if (gIsScalingCamera) { float y1 = gWindowHeight - gLastMouseY; float y2 = gWindowHeight - y; gCameraScale *= 1 + (y1 - y2) / gWindowHeight; updateCamera(); } gLastMouseX = x; gLastMouseY = y; }
void RIMLS::Fit() { double f = 0; Vec3d grad_f(0, 0, 0); do { int i = 0; do { double sumW, sumF; sumW = sumF = 0; Vec3d sumGW, sumGF, sumN; sumGW = sumGF = sumN = Vec3d(0.0, 0.0, 0.0); for (DataPoint& p : m_neighbors) { Vec3d px = m_x - p.pos(); double fx = px.dot(p.normal()); double alpha = 1.0; if (i > 0) { alpha = exp(-pow((fx - f) / m_sigmaR, 2)) * exp(-pow((p.normal() - grad_f).norm() / m_sigmaN, 2)); } double phi = exp(-pow(px.norm() / m_sigmaT, 2)); double w = alpha*phi; Vec3d grad_w = -2.0*alpha*phi*px / pow(m_sigmaT, 2); sumW += w; sumGW += grad_w; sumF += w*fx; sumGF += grad_w*fx; sumN += w*p.normal(); } f = sumF / sumW; grad_f = (sumGF - f*sumGW + sumN) / sumW; } while (++i<m_iter && !convergence()); m_x -= f*grad_f; } while ((f*grad_f).norm() > m_threshold); }
void SofaHAPIHapticsDevice::draw(const sofa::core::visual::VisualParams* vparams) { if (!device.get()) return; if(drawDevice.getValue()) { // TODO HAPI::HAPIHapticsDevice::DeviceValues dv = device->getDeviceValues(); /// COMPUTATION OF THE virtualTool 6D POSITION IN THE World COORDINATES Vec3d pos = conv(dv.position); Vec3d force = conv(dv.force); Quat quat = conv(dv.orientation); quat.normalize(); Transform baseDevice_H_endDevice(pos*data.scale, quat); Transform world_H_virtualTool = data.world_H_baseDevice * baseDevice_H_endDevice * data.endDevice_H_virtualTool; Vec3d wpos = world_H_virtualTool.getOrigin(); vparams->drawTool()->setLightingEnabled(true); //Enable lightning if (drawHandleSize.getValue() == 0.0f) { std::vector<Vec3d> points; points.push_back(wpos); vparams->drawTool()->drawSpheres(points, 1.0f, sofa::defaulttype::Vec<4,float>(0,0,1,1)); } else { Vec3d wposH = wpos + data.world_H_baseDevice.projectVector( baseDevice_H_endDevice.projectVector(Vec3d(0.0,0.0,drawHandleSize.getValue()))); vparams->drawTool()->drawArrow(wposH, wpos, drawHandleSize.getValue()*0.05f, sofa::defaulttype::Vec<4,float>(0,0,1,1)); } if (force.norm() > 0 && drawForceScale.getValue() != 0.0f) { //std::cout << "F = " << force << std::endl; Vec3d fscaled = force*(drawForceScale.getValue()*data.scale); Transform baseDevice_H_endDeviceF(pos*data.scale+fscaled, quat); Transform world_H_virtualToolF = data.world_H_baseDevice * baseDevice_H_endDeviceF * data.endDevice_H_virtualTool; Vec3d wposF = world_H_virtualToolF.getOrigin(); vparams->drawTool()->drawArrow(wpos, wposF, 0.1f, sofa::defaulttype::Vec<4,float>(1,0,0,1)); } vparams->drawTool()->setLightingEnabled(false); //Disable lightning } }
double DistWeightFunc::w(const Vec3d& _q) const { double d = _q.norm(); return (d <= m_t) ? m_wk->f(d / m_t) : 0.0; }
double DistWeightFunc::scaledw(const Vec3d& _q) const { double d = _q.norm(); return (d <= m_t) ? (-d*m_wk->df(d / m_t) / (m_t*m_t)) : 0.0; }
std::vector<Vec3d> ClosedPolygon::getEqualDistancePoints(int numSides, const Vec3d& center) { std::vector<Vec3d> result; int N = closedPoints.size(); if(N < 1) return result; // empty polygon for(int i = 0; i < N; i++) lines.push_back(Line(closedPoints[i], closedPoints[(i+1) % N], i)); this->computeLengths(); // Distance to walk on polygon double segmentLength = this->closedLength / numSides; // Locate start point using vecUp Vec3d startPoint; int startIndex = 0; Plane halfPlane(vecUp, center); //testPlanes1.push_back(halfPlane); double minDist = DBL_MAX; // Test intersection with all lines and remember minimum one for(int i = 0; i < N; i++) { Vec3d pointIntersect; int res = halfPlane.LineIntersect(lines[i], pointIntersect); if(res == INTERSECT || res == ENDPOINT_INTERSECT) { Vec3d toIntsect = pointIntersect - center; if(toIntsect.norm() < minDist && dot(toIntsect, vecB) > 0) { minDist = toIntsect.norm(); startPoint = pointIntersect; startIndex = i; } } } double t = lines[startIndex].timeAt(startPoint); int index = startIndex; // Compute equal-dist points on polygon for(int s = 0; s < numSides; s++) { // Add new point result.push_back(lines[index].pointAt(t)); walk(segmentLength, t, index, &t, &index); } // if polygon is opposite direction then reverse if( signedArea(result, plane.n, center) < 0 ) { std::reverse(result.begin(), result.end()); std::rotate(result.begin(), result.begin()+result.size()-1 , result.end()); } return closedPoints = result; }
bool ConvexHull3::getExtremes( std::vector<int> &mExtreme ) { bool mExtremeCCW = false; mExtreme.resize(4, 0); int nbPnts = mPnts.size(); // Compute the axis-aligned bounding box for the input mPnts. Keep track // of the indices into 'mPnts' for the current min and max. Vec3d mMin, mMax; int j, indexMin[3], indexMax[3]; for (j = 0; j < 3; ++j) { mMin[j] = mPnts[0][j]; mMax[j] = mMin[j]; indexMin[j] = 0; indexMax[j] = 0; } int i; for (i = 1; i < nbPnts; ++i) { for (j = 0; j < 3; ++j) { if (mPnts[i][j] < mMin[j]) { mMin[j] = mPnts[i][j]; indexMin[j] = i; } else if (mPnts[i][j] > mMax[j]) { mMax[j] = mPnts[i][j]; indexMax[j] = i; } } } // Determine the maximum range for the bounding box. Real mMaxRange = mMax[0] - mMin[0]; mExtreme[0] = indexMin[0]; mExtreme[1] = indexMax[0]; Real range = mMax[1] - mMin[1]; if (range > mMaxRange) { mMaxRange = range; mExtreme[0] = indexMin[1]; mExtreme[1] = indexMax[1]; } range = mMax[2] - mMin[2]; if (range > mMaxRange) { mMaxRange = range; mExtreme[0] = indexMin[2]; mExtreme[1] = indexMax[2]; } // The origin is either the point of minimum x-value, point of // minimum y-value, or point of minimum z-value. Vec3d mOrigin = mPnts[mExtreme[0]]; // Test whether the point set is (nearly) a line segment. std::vector<Vec3d> mDirection(3, Vec3d()); mDirection[0] = mPnts[mExtreme[1]] - mOrigin; mDirection[0].normalize(); Real maxDistance = (Real)0; Real distance, Dot; mExtreme[2] = mExtreme[0]; for (i = 0; i < nbPnts; ++i) { Vec3d diff = mPnts[i] - mOrigin; Dot = dot(mDirection[0], diff); Vec3d proj = diff - Dot*mDirection[0]; distance = proj.norm(); if (distance > maxDistance) { maxDistance = distance; mExtreme[2] = i; } } // Test whether the point set is (nearly) a planar polygon. mDirection[1] = mPnts[mExtreme[2]] - mOrigin; Dot = dot(mDirection[0], mDirection[1]); mDirection[1] -= Dot*mDirection[0]; mDirection[1].normalize(); mDirection[2] = cross(mDirection[0], mDirection[1]); maxDistance = (Real)0; Real maxSign = (Real)0; mExtreme[3] = mExtreme[0]; for (i = 0; i < nbPnts; ++i) { Vec3d diff = mPnts[i] - mOrigin; distance = dot(mDirection[2], diff); if (fabs(distance) > maxDistance) { maxDistance = fabs(distance); mExtremeCCW = (distance > (Real)0); mExtreme[3] = i; } } return mExtremeCCW; }