double angleBetweenVectors( const SbVec3f& u, const SbVec3f& v ) { double cosAngle = v.dot( u ) / ( u.length() * v.length() ); return acos( cosAngle ); }
void ViewProviderMeasureDistance::updateData(const App::Property* prop) { if (prop->getTypeId() == App::PropertyVector::getClassTypeId() || prop == &Mirror || prop == &DistFactor) { if (strcmp(prop->getName(),"P1") == 0) { Base::Vector3d v = static_cast<const App::PropertyVector*>(prop)->getValue(); pCoords->point.set1Value(0, SbVec3f(v.x,v.y,v.z)); } else if (strcmp(prop->getName(),"P2") == 0) { Base::Vector3d v = static_cast<const App::PropertyVector*>(prop)->getValue(); pCoords->point.set1Value(1, SbVec3f(v.x,v.y,v.z)); } SbVec3f pt1 = pCoords->point[0]; SbVec3f pt2 = pCoords->point[1]; SbVec3f dif = pt1-pt2; float length = fabs(dif.length())*DistFactor.getValue(); if (Mirror.getValue()) length = -length; if (dif.sqrLength() < 10.0e-6f) { pCoords->point.set1Value(2, pt1+SbVec3f(0.0f,0.0f,length)); pCoords->point.set1Value(3, pt2+SbVec3f(0.0f,0.0f,length)); } else { SbVec3f dir = dif.cross(SbVec3f(1.0f,0.0f,0.0f)); if (dir.sqrLength() < 10.0e-6f) dir = dif.cross(SbVec3f(0.0f,1.0f,0.0f)); if (dir.sqrLength() < 10.0e-6f) dir = dif.cross(SbVec3f(0.0f,0.0f,1.0f)); dir.normalize(); if (dir.dot(SbVec3f(0.0f,0.0f,1.0f)) < 0.0f) length = -length; pCoords->point.set1Value(2, pt1 + length*dir); pCoords->point.set1Value(3, pt2 + length*dir); } SbVec3f pos = (pCoords->point[2]+pCoords->point[3])/2.0f; pTranslation->translation.setValue(pos); std::stringstream s; s.precision(3); s.setf(std::ios::fixed | std::ios::showpoint); s << dif.length(); pLabel->string.setValue(s.str().c_str()); } ViewProviderDocumentObject::updateData(prop); }
double angleBetweenVectors( const SbVec3f& u, const SbVec3f& v, const SbVec3f& normal ) { double cosAngle = v.dot( u ) / ( u.length() * v.length() ); float angle = acos( cosAngle ); // FIXME: Check if this is required // SbVec3f cross = u.cross( v ); if( cross.dot( normal ) < 0 ) angle = 2 * M_PI - angle; // return angle; }
SbBool SoXipPolygon::canClose( const SbVec3f& pos ) const { // Do not need to close the shape if already closed if( isClosed() ) return FALSE; // The number of points should be at least 4. // The 4th point corresponds to the one which is moving. if( point.getNum() > 3 ) { // minimum size for contour is given, check if the last point // is very close to the start point so we can close the contour SbVec3f screenPt[2]; mViewVolume.projectToScreen( point[0], screenPt[0] ); mViewVolume.projectToScreen( pos, screenPt[1] ); SbVec3f d = screenPt[0] - screenPt[1]; d[0] *= mViewport.getViewportSizePixels()[0]; d[1] *= mViewport.getViewportSizePixels()[1]; d[2] = 0; return d.length() < CLOSING_MIN_PIXEL_DISTANCE; } return FALSE; }
/*! \COININTERNAL Called when dragger is selected (picked) by the user. */ void SoRotateSphericalDragger::dragStart(void) { SoSwitch *sw; sw = SO_GET_ANY_PART(this, "rotatorSwitch", SoSwitch); SoInteractionKit::setSwitchValue(sw, 1); sw = SO_GET_ANY_PART(this, "feedbackSwitch", SoSwitch); SoInteractionKit::setSwitchValue(sw, 1); SbVec3f hitPt = this->getLocalStartingPoint(); float radius = hitPt.length(); this->sphereProj->setSphere(SbSphere(SbVec3f(0.0f, 0.0f, 0.0f), radius)); this->sphereProj->setViewVolume(this->getViewVolume()); this->sphereProj->setWorkingSpace(this->getLocalToWorldMatrix()); switch (this->getFrontOnProjector()) { case FRONT: this->sphereProj->setFront(TRUE); break; case BACK: this->sphereProj->setFront(TRUE); break; default: // avoid warnings case USE_PICK: this->sphereProj->setFront(this->sphereProj->isPointInFront(hitPt)); break; } SbVec3f projPt = this->sphereProj->project(this->getNormalizedLocaterPosition()); this->getLocalToWorldMatrix().multVecMatrix(projPt, this->prevWorldHitPt); this->prevMotionMatrix = this->getMotionMatrix(); }
void rotateCamera(const SbRotation &rot) { SoCamera * camera = viewer->getCamera(); // get center of rotation const float radius = camera->focalDistance.getValue(); SbVec3f forward; camera->orientation.getValue().multVec(SbVec3f(0,0,-1), forward); const SbVec3f center = camera->position.getValue() + radius * forward; // apply new rotation to the camera camera->orientation = rot * camera->orientation.getValue(); // reposition camera to look at pt of interest camera->orientation.getValue().multVec(SbVec3f(0,0,-1), forward); camera->position = center - radius * forward; headlightRot->rotation = camera->orientation.getValue(); // Adjust clipping planes SoGetBoundingBoxAction clipbox_action(getViewportRegion()); clipbox_action.apply(viewer->getSceneRoot()); SbBox3f bbox = clipbox_action.getBoundingBox(); if (bbox.isEmpty()) return; SbSphere bSphere; bSphere.circumscribe(bbox); float denumerator = forward.length(); float numerator = (bSphere.getCenter() - camera->position.getValue()).dot(forward); float distToCenter = (forward * (numerator / denumerator)).length(); float farplane = distToCenter + bSphere.getRadius(); // if scene is behind the camera, don't change the planes if (farplane < 0) return; float nearplane = distToCenter - bSphere.getRadius(); if (nearplane < (0.001 * farplane)) nearplane = 0.001 * farplane; camera->nearDistance = nearplane; camera->farDistance = farplane; }
// Doc in parent SbBool SoVRMLSphereSensor::dragStart(void) { SbVec3f thehitpt = this->getLocalStartingPoint(); float radius = thehitpt.length(); if (radius == 0.0f) return FALSE; this->sphereproj->setSphere(SbSphere(SbVec3f(0.0f, 0.0f, 0.0f), radius)); this->sphereproj->setViewVolume(this->getViewVolume()); this->sphereproj->setWorkingSpace(this->getLocalToWorldMatrix()); this->getLocalToWorldMatrix().multVecMatrix(thehitpt, this->prevworldhitpt); this->prevrotation = SbRotation::identity(); this->rotation_changed = this->offset.getValue(); return TRUE; }
const SbVec4f & SoTextureCoordinateEnvironment::valueCallback(void *action, const SbVec3f &point, const SbVec3f &normal) // //////////////////////////////////////////////////////////////////////// { SoAction *a = (SoAction *)action; // // See the glTexGen() man page for the math here. // // First, map normal and point into eye space: const SbMatrix &mm = SoModelMatrixElement::get(a->getState()); const SbMatrix &vm = SoViewingMatrixElement::get(a->getState()); // Compute the matrix that transforms normals from object-space to // eye-space; use the inverse transpose to scale correctly SbVec3f normalE; SbMatrix nm = (vm * mm).inverse().transpose(); nm.multDirMatrix(normal, normalE); SbVec3f pointE; mm.multVecMatrix(point, pointE); // Gives world-space point vm.multVecMatrix(pointE, pointE); // ... to eye-space. // Get the normalized vector from the eye (which is conveniently // at 0,0,0 in eye space) to the point. pointE.normalize(); // Now, figure out reflection vector, from formula: // R = P - 2 (N . N) pointE SbVec3f reflection = pointE - 2.0 * normalE.dot(normalE) * pointE; // Finally, compute s/t coordinates... reflection[2] += 1.0; float magnitude = reflection.length(); // This is static so we can return a reference to it static SbVec4f result; result.setValue(reflection[0] / magnitude + 0.5, reflection[1] / magnitude + 0.5, 0.0, 1.0); return result; }
SbBool SoXipPolygon::isConsistent() const { // check if the contour size is greater than the minimum size. const SbVec3f* pointPtr = point.getValues(0); SbBox3f bbox; for( int i = 0; i < point.getNum(); ++ i ) bbox.extendBy( pointPtr[i] ); SbVec3f bbSize; bbox.getSize(bbSize[0], bbSize[1], bbSize[2]); float screenScale = mViewVolume.getHeight() / mViewport.getViewportSizePixels()[1]; float bbLengthPix = bbSize.length() / screenScale; return ( bbLengthPix >= (2 * CLOSING_MIN_PIXEL_DISTANCE) ); }
void ShapeData::triangleCB(void * closure, SoCallbackAction *, const SoPrimitiveVertex * v1, const SoPrimitiveVertex * v2, const SoPrimitiveVertex * v3) { PrimitiveData * primitives = static_cast<PrimitiveData *>(closure); const SbVec3f & oa = v1->getPoint(); const SbVec3f & ob = v2->getPoint(); const SbVec3f & oc = v3->getPoint(); SbVec3f wa, wb, wc; primitives->transform.multVecMatrix(oa, wa); primitives->transform.multVecMatrix(ob, wb); primitives->transform.multVecMatrix(oc, wc); // Only add valid triangles. const SbVec3f normal = (wa - wb).cross(wa - wc); if (normal.length() > 0.0f) { SbTri3f * triangle = new SbTri3f(wa, wb, wc); primitives->addTriangle(triangle); } else { static SbBool warn = TRUE; if (warn) { warn = FALSE; SoDebugError::postWarning("ShapeData::triangleCB", "Found an invalid triangle while souping up " "triangle primitives from a shape for " "intersection testing. Transformed==" "<<%f, %f, %f>, <%f, %f, %f>, <%f, %f, %f>>. " "Untransformed==" "<<%f, %f, %f>, <%f, %f, %f>, <%f, %f, %f>>. " "Will only warn once, there could be more " "cases.", wa[0], wa[1], wa[2], wb[0], wb[1], wb[2], wc[0], wc[1], wc[2], oa[0], oa[1], oa[2], ob[0], ob[1], ob[2], oc[0], oc[1], oc[2]); } } }
// Write SbRotation to output stream. Used from SoSFRotation and // SoMFRotation. void sosfrotation_write_value(SoOutput * out, const SbRotation & r) { SbVec3f axis; float angle; r.getValue(axis, angle); // Handle invalid rotation specifications. if (axis.length() == 0.0f) { axis.setValue(0.0f, 0.0f, 1.0f); angle = 0.0f; } out->write(axis[0]); if(!out->isBinary()) out->write(' '); out->write(axis[1]); if(!out->isBinary()) out->write(' '); out->write(axis[2]); if(!out->isBinary()) out->write(" "); out->write(angle); }
void SoSpring::applyForce() { // Calculate the forces according to Hooke's Law // k * (DISTij-LENij) * Vij SbVec3f resultingForces; SbVec3f deltaVector = endpos.getValue()-startpos.getValue(); float elongation; SbVec3f acc1 = acc.getValue(); SbVec3f vel1 = vel.getValue(); elongation = deltaVector.length(); deltaVector.normalize(); resultingForces=stiffness.getValue()*elongation*deltaVector; // debug // printf("a1%f,a2%f,a3%f, v1%f,v2%f,v3%f,p1%f,p2%f,p3%f \n", acc1[0],acc1[1],acc1[2],vel1[0],vel1[1],vel1[2],startpos.getValue()[0],startpos.getValue()[1],startpos.getValue()[2] ); acc1 = resultingForces*(1.0f/mass.getValue()); vel1 = vel1 + (acc1*timeStep.getValue()); vel1 *= damp.getValue(); vel.setValue(vel1); acc.setValue(acc1); if ( (vel1.length()<treshold.getValue())& (acc1.length()<treshold.getValue() ) ) { // debug //printf("INFO: treasold reached!! \n"); timer->unschedule(); } else // update { startpos.setValue(startpos.getValue()+ vel1*timeStep.getValue()) ; } }
/// Override original method since it seems to adjust the clipping planes in a weird manner. /// Maybe using a screen-space projection or whatever. virtual void adjustCameraClippingPlanes() { SoCamera * camera = getCamera(); if (!camera) return; SoGetBoundingBoxAction clipbox_action(getViewportRegion()); clipbox_action.apply(getSceneRoot()); SbBox3f bbox = clipbox_action.getBoundingBox(); if (bbox.isEmpty()) return; SbSphere bSphere; bSphere.circumscribe(bbox); SbVec3f forward; camera->orientation.getValue().multVec(SbVec3f(0,0,-1), forward); float denumerator = forward.length(); float numerator = (bSphere.getCenter() - camera->position.getValue()).dot(forward); float distToCenter = (forward * (numerator / denumerator)).length(); float farplane = distToCenter + bSphere.getRadius(); // if scene is behind the camera, don't change the planes if (farplane < 0) return; float nearplane = distToCenter - bSphere.getRadius(); if (nearplane < (0.001 * farplane)) nearplane = 0.001 * farplane; camera->nearDistance = nearplane; camera->farDistance = farplane; }
bool XipGeomUtils::intersect(const SbMatrix &mpr, const SbMatrix &model, SbMatrix &newMpr, int width, int height, int depth, bool shifted) { SbVec3f t, s ; SbRotation r, so ; model.getTransform ( t, r, s, so ) ; SbVec3f vsize, origin; SbVec3f axisX, axisY, axisZ; axisX[0] = model[0][0]; axisX[1] = model[0][1]; axisX[2] = model[0][2]; axisX.normalize(); axisY[0] = model[1][0]; axisY[1] = model[1][1]; axisY[2] = model[1][2]; axisY.normalize(); axisZ[0] = model[2][0]; axisZ[1] = model[2][1]; axisZ[2] = model[2][2]; axisZ.normalize(); if ( shifted ) { vsize[0] = s[0] / static_cast<float>(width); vsize[1] = s[1] / static_cast<float>(height); vsize[2] = s[2] / static_cast<float>(depth); origin = t - 0.5f*vsize[0]*axisX - 0.5f*vsize[1]*axisY - 0.5f*vsize[2]*axisZ; } else { origin[0] = t[0]; origin[1] = t[1]; origin[2] = t[2]; } axisX[0] = model[0][0]; axisX[1] = model[0][1]; axisX[2] = model[0][2]; axisY[0] = model[1][0]; axisY[1] = model[1][1]; axisY[2] = model[1][2]; axisZ[0] = model[2][0]; axisZ[1] = model[2][1]; axisZ[2] = model[2][2]; SbVec3f vert[8]; vert[0] = origin; vert[1] = origin + axisX; vert[2] = origin + axisX + axisY; vert[3] = origin + axisY; vert[4] = vert[0] + axisZ; vert[5] = vert[1] + axisZ; vert[6] = vert[2] + axisZ; vert[7] = vert[3] + axisZ; mpr.getTransform ( t, r, s, so ) ; axisX[0] = mpr[0][0]; axisX[1] = mpr[0][1]; axisX[2] = mpr[0][2]; axisX.normalize(); axisY[0] = mpr[1][0]; axisY[1] = mpr[1][1]; axisY[2] = mpr[1][2]; axisY.normalize(); SbPlane plane(t, t+axisX, t+axisY); vector<SbVec3f> pts; pts.clear(); SbVec3f pt; for (int i=0; i<12; ++i) { if ( intersect(vert[edge[i][0]], vert[edge[i][1]], plane, pt) ) { pts.push_back(pt); } } for (int i=0; i<8; ++i) { if ( isOnPlane(vert[i], plane) ) { pts.push_back(vert[i]); } } if ( pts.size()<3 ) return false; SbVec3f u; SbVec2f minv(numeric_limits<float>::max(), numeric_limits<float>::max()); SbVec2f maxv(-numeric_limits<float>::max(), -numeric_limits<float>::max()); float tmp; for (size_t i=0; i<pts.size(); ++i) { u = pts[i] - t; tmp = u.dot(axisX); if ( minv[0]>tmp ) minv[0] = tmp; if ( maxv[0]<tmp ) maxv[0] = tmp; tmp = u.dot(axisY); if ( minv[1]>tmp ) minv[1] = tmp; if ( maxv[1]<tmp ) maxv[1] = tmp; } SbVec2f center = 0.5f*(minv+maxv); origin = t + center[0]*axisX + center[1]*axisY; if ( !XipGeomUtils::isOnPlane(origin, mpr) ) { XipDebug::output("Error: new center is not on the plane"); } SbVec3f pt0 = t + minv[0]*axisX + minv[1]*axisY; SbVec3f pt1 = t + maxv[0]*axisX + minv[1]*axisY; SbVec3f pt2 = t + maxv[0]*axisX + maxv[1]*axisY; float scale = (pt1 - pt0).length(); u = pt2 - pt1; if ( scale<u.length() ) scale = u.length(); scale *= 1.01f; newMpr.setTransform(origin, r, SbVec3f(scale, scale, scale), so); //XipDebug::output("%f --> %f", s[0], scale); return true; }