int main(void) { const int n =6; // 4I , 2 lambda int info; // system params Coil coil; Magnet magnet; coil.d = .0575; coil.R = 0.075; magnet.x = 0.01; magnet.y = 0.0; magnet.Fx = 1 * pow(10,-8); magnet.Fy = 1.* pow(10,-8); magnet.gamma = 35.8 * pow(10,-6); double th = -19 * PI/180; // convert angle in degrees to radians double Bmag = 10. * pow(10,-6); double Bx = Bmag * cos(th); double By = Bmag * sin(th); Vector2d mvec(Bx,By); mvec = magnet.gamma/sqrt(Bx * Bx + By * By)*mvec; cout << "mvec" << endl << mvec << endl; MatrixXd Bmat = computeBmat(magnet.x,magnet.y,coil.R,coil.d); cout << "Bmat" << endl << Bmat << endl; //MatrixXd Matrix4d A; cout << "A" << endl << A << endl; A.block(0,0,2,4) = Bmat; cout << "A row 1-2" << endl << A << endl; A.block(2,0,1,4) = mvec.transpose() * Dx(magnet.x,magnet.y,coil.R,coil.d); A.block(3,0,1,4) = mvec.transpose() * Dy(magnet.x,magnet.y,coil.R,coil.d); cout << "A " << endl << A << endl; cout << "A inverse is:" << endl << A.inverse() << endl; Vector4d BF; BF << Bx,By,magnet.Fx,magnet.Fy; cout << "BF: " << endl << BF << endl; Vector4d Isolve = A.inverse() * BF; cout << "Isolve: " << endl << Isolve << endl; return 0; }
void VRMolecule::rotateBond(int a, int b, float f) { if (atoms.count(a) == 0) return; if (atoms.count(b) == 0) return; VRAtom* A = atoms[a]; VRAtom* B = atoms[b]; uint now = VRGlobals::CURRENT_FRAME + rand(); A->recFlag = now; Vec3d p1 = Vec3d( A->getTransformation()[3] ); Vec3d p2 = Vec3d( B->getTransformation()[3] ); Vec3d dir = p2-p1; Quaterniond q(dir, f); Matrix4d R; R.setRotate(q); Matrix4d T; T[3] = B->getTransformation()[3]; Matrix4d _T; T.inverse(_T); T.mult(R); T.mult(_T); //cout << "ROTATE bound " << a << "-" << b << " around " << dir << " with " << f << endl; B->propagateTransformation(T, now); updateGeo(); }
ClothoidPtr ClothoidFitter::getCurve() const { Matrix4d lhs; lhs = _getLhs(_totalLength); Vector4d abcd = lhs.inverse() * _rhs; return getClothoidWithParams(abcd); }
bool calcLegAngles(double** out_q) { rpy = rpyFromRot(R); cy = cos(rpy[2]); sy = sin(rpy[2]); cp = cos(rpy[1]); sp = sin(rpy[1]); cr = cos(rpy[0]); sr = sin(rpy[0]); Vector3 O1(0.0, sign * robot->l0, -robot->l1); const Vector3& F = p; Vector3 V1 = F - O1; Vector3 XF(cy * cp, sy * cp, -sp); Vector3 V1xXF = V1.cross(XF); Vector3 Z2 = -sign * V1xXF / V1xXF.norm(); q[0] = atan2(-sign * Z2[0], sign * Z2[1]); q[1] = asin(-sign * Z2[2]); double c1 = cos(q[0]); double s1 = sin(q[0]); double c2 = cos(q[1]); double s2 = sin(q[1]); double s1s2 = s1 * s2; double c1s2 = c1 * s2; slo1 = sign * robot->lo1; TB2 << s1s2, -sign * c1, -sign * s1 * c2, slo1 * s1 + robot->l2 * c1 + robot->lo2 * s1s2, -c1s2, -sign * s1, sign * c1 * c2, sign * robot->l0 - slo1 * c1 + robot->l2 * s1 - robot->lo2 * c1s2, -c2, 0.0, -sign * s2, -robot->l1 - robot->lo2 * c2, 0.0, 0.0, 0.0, 1.0; Vector3 V2 = (TB2.inverse() * Vector4d(F[0], F[1], F[2], 1.0)).head(3); double D = (V2.squaredNorm() - robot->l3 * robot->l3 - robot->l4 * robot->l4) / (2.0 * robot->l3 * robot->l4); if(fabs(D) > 1.0){ return false; } q[3] = atan2(-sign * sqrt(1.0 - D * D), D); double c4 = cos(q[3]); double s4 = sin(q[3]); double beta = atan2(-V2[1], sqrt(V2[0] * V2[0] + V2[2] * V2[2])); double alpha = atan2(robot->l4 * s4, robot->l3 + robot->l4 * c4); q[2] = -(beta - alpha); q[3] = -q[3]; double c3 = cos(q[2]); double s3 = sin(q[2]); double q2q3 = q[2] + q[3]; double c34 = cos(q2q3); double s34 = sin(q2q3); Matrix4d T24; T24 << c34, s34, 0, robot->l3 * c3 + robot->l4 * c34, s34, -c34, 0, robot->l3 * s3 + robot->l4 * s34, 0.0, 0.0, -1, 0.0, 0.0, 0.0, 0, 1.0; TB4.noalias() = TB2 * T24; double spsr = sp * sr; double spcr = sp * cr; TBF << cy * cp, -sy * cr + cy * spsr, sy * sr + cy * spcr, p.x(), sy * cp, cy * cr + sy * spsr, -cy * sr + sy * spcr, p.y(), -sp, cp * sr, cp * cr, p.z(), 0, 0, 0, 1.0; Matrix4d T4F; T4F.noalias() = TB4.inverse() * TBF; q[4] = atan2(-sign * T4F(0,0), sign * T4F(1,0)); q[5] = atan2( sign * T4F(2,2), -sign * T4F(2,1)); // Numerical refining TB0 << 0.0, -1.0, 0.0, 0.0, 1.0, 0.0, 0.0, sign * robot->l0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; Af << 0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; bool solved = false; int i; for(i=0; i < 30; ++i){ if(calcEndPositionDifference()){ solved = true; break; } // Jacobian Calculation Vector3 Z0 = TB0.block<3,1>(0,2); Vector3 Z1 = TB1.block<3,1>(0,2); Vector3 Z2 = TB2.block<3,1>(0,2); Vector3 Z3 = TB3.block<3,1>(0,2); Vector3 Z4 = TB4.block<3,1>(0,2); Vector3 Z5 = TB5.block<3,1>(0,2); Vector3 O0 = TB0.block<3,1>(0,3); Vector3 O1 = TB1.block<3,1>(0,3); Vector3 O2 = TB2.block<3,1>(0,3); Vector3 O3 = TB3.block<3,1>(0,3); Vector3 O4 = TB4.block<3,1>(0,3); Vector3 O5 = TB5.block<3,1>(0,3); Vector3 O6 = TB6.block<3,1>(0,3); Matrix6 J; J << Z0.cross(O6 - O0), Z1.cross(O6 - O1), Z2.cross(O6 - O2), Z3.cross(O6 - O3), Z4.cross(O6 - O4), Z5.cross(O6 - O5), Z0, Z1, Z2, Z3, Z4, Z5 ; // Levenberg-Marquardt Method const double lambda = 0.001; Matrix6 C; C.noalias() = J.transpose() * (J * J.transpose() + Matrix6::Identity() * lambda * lambda).inverse(); dq.noalias() = C * dp; q += dq; if(dq.norm() <= 1.0e-5){ break; } } if(!solved){ solved = calcEndPositionDifference(); } if(solved){ *out_q[0] = q[0]; *out_q[1] = q[1]; *out_q[2] = q[2] + sign * robot->q2q3offset; *out_q[3] = q[3] - sign * robot->q2q3offset; *out_q[4] = q[4]; *out_q[5] = q[5]; } return solved; }
// Given the solution of the equation up to the time step solution.timestep - 1, computes the solution at the sime step solution.timestep and increases timestep by one void step(double * data, Solution & solution, const SimulationContext & context, const vector<shared_ptr<SimulationResult>> & results) { // The equation is result = (2-Re dt)^(-1) (2 + Re dt) se + (2-Re dt)^(-1) G s // Total Spin Vector4d s = 0.5*(3.0*solution.current.total.spin - solution.previous.total.spin); // Aliases const Settings & settings = context.settings(); double dt = data[Index::Duration] / data[Index::TimestepCount]; data[Index::Timestep] = solution.timeStep; data[Index::Time] = solution.timeStep * data[Index::Duration] / data[Index::TimestepCount]; context.evaluateFunctions(ComputeFor::Step, Compute::OnceForEachStep, data); // test double h = accumulate(solution.current.spin.begin(), solution.current.spin.end(), 0.0, [](double old, const Vector4d & s) {return old + abs(s(2));}) / solution.current.spin.size(); (void)h; // Solving the equation of motion for the individual spins for(unsigned int i = 0; i < settings.energySamples().size(); ++i) { Vector4d & spin = solution.current.spin.at(i); data[Index::EnergyIndex] = i; data[Index::Energy] = settings.energySamples().at(i); context.evaluateFunctions(ComputeFor::Step, Compute::OnceForEachEnergy, data); // Matrix initialization Matrix4d R, RD; // R = RF + RE + RD + RL; // Pseudo magnetic field /* 0, -data[Index::Frequency], 0, 0, * data[Index::Frequency], 0, 0, 0, * 0, 0, 0, 0, * 0, 0, 0, 0; */ R.setZero(); R(0,1) += -data[Index::Frequency]; R(1,0) += +data[Index::Frequency]; // Exchange /* 0, -data[Index::Exchange]*s(2)*s(3), data[Index::Exchange]*s(1)*s(3), 0, * data[Index::Exchange]*s(2)*s(3), 0, -data[Index::Exchange]*s(0)*s(3), 0, * -data[Index::Exchange]*s(1)*s(3), data[Index::Exchange]*s(0)*s(3), 0, 0, * 0, 0, 0, 0; */ R(0,1) += -data[Index::Exchange]*s(2)*s(3); R(0,2) += +data[Index::Exchange]*s(1)*s(3); R(1,0) += +data[Index::Exchange]*s(2)*s(3); R(1,2) += -data[Index::Exchange]*s(0)*s(3); R(2,0) += -data[Index::Exchange]*s(1)*s(3); R(2,1) += +data[Index::Exchange]*s(0)*s(3); // Damping /* -data[Index::Damping], 0, 0, 0, * 0, -data[Index::Damping], 0, 0, * 0, 0, -data[Index::Damping], 0, * 0, 0, 0, 0; */ RD.setZero(); RD(0,0) += -data[Index::Damping]; RD(1,1) += -data[Index::Damping]; RD(2,2) += -data[Index::Damping]; R += RD; // Atom losses double dfls = 0.25 * (data[Index::AtomLossFromF1] + data[Index::AtomLossFromF2]); // Total atom losses to spectator states double dfld = 0.25 * (data[Index::AtomLossFromF1] - data[Index::AtomLossFromF2]); // Difference of atom losses to spectator states /* 0, 0, 0, 0, * 0, 0, 0, 0, * 0, 0, -(dfls*s(3) + dfld*s(2)), -(dfld*s(3) + dfls*s(2)), * 0, 0, -(dfld*s(3) + dfls*s(2)), -(dfls*s(3) + dfld*s(2));*/ R(2,2) += -(dfls*s(3) + dfld*s(2)); R(2,3) += -(dfld*s(3) + dfls*s(2)); R(3,2) += -(dfld*s(3) + dfls*s(2)); R(3,3) += -(dfls*s(3) + dfld*s(2)); // Computation Matrix4d RT; // = 2 + R * dt Matrix4d RI; // = (2 - R * dt).inverse(); // We can't inverse in place, so we use RT as a temporary to compute RI RT = 2.0 * Matrix4d::Identity() - dt * R; RI = RT.inverse(); RT = 2.0 * Matrix4d::Identity() + dt * R; spin = RI * (RT * spin - dt * RD * s).eval(); } // Update values at the previous timestep solution.previous.total.spin = solution.current.total.spin; solution.previous.total.phase = solution.current.total.phase; for(unsigned int i = 0; i < settings.energyBins().size(); ++i) { solution.previous.partial(i).phase = solution.current.partial(i).phase; } // Update current values solution.current.total.spin = accumulate(solution.current.spin.begin(), solution.current.spin.end(), Vector4d::Zero().eval()) / solution.current.spin.size(); // TODO : Move inside the loop solution.current.total.atomLosses(1) += dt * data[Index::AtomLossFromF1]* 0.5 * (s(3) + s(2)); solution.current.total.atomLosses(2) += dt * data[Index::AtomLossFromF2]* 0.5 * (s(3) - s(2)); // Partial spins for(unsigned int i = 0; i < settings.energyBins().size(); ++i) { const Settings::EnergyBin & bin = settings.energyBins().at(i); solution.current.partial(i).spin = accumulate(solution.current.spin.begin() + bin.first, solution.current.spin.begin() + bin.second, Vector4d::Zero().eval()) / (bin.second - bin.first); } // Computes the phase of the spins phase(solution, settings); // Save the data if necessary save(data, solution, settings, results); ++solution.timeStep; }
void BoneActor::doIK(Vector3f loc, bool bAbsolute){ /********************************************************************************* 3 part process: 1. gather all necessary numbers 2. rotate the system into target plane (plane between target point and shoulder) 3. do 2D IK *********************************************************************************/ //leave if we don't have two parents if (!base || !base->base ){ cout << "no double-base!!! Cannot do IK" << endl; Actor::setLocation(loc); return; } //for debug stuff f+=0.01; ///clear transform Matrices! Matrix4d initialTransform; initialTransform=base->base->transformMatrix; base->base->transformMatrix.identity(); base->transformMatrix.identity(); transformMatrix.identity(); base->base->baseMatrix=calcMatrix(base->base); base->baseMatrix=calcMatrix(base); baseMatrix=calcMatrix(this); ///gathering numbers Vector3f currentUpperVec, currentLowerVec, initialUpperVec, initialLowerVec; MeshData* myMesh=renderer->vboList[parent->vboMeshID]; //find original angles for non-aligned bones - from mesh data! for (int i=0;i<(int)myMesh->bones.size();i++){ if (myMesh->bones[i]->name==base->name) initialUpperVec=myMesh->bones[i]->boneMatrix->getTranslation(); if (myMesh->bones[i]->name==name) initialLowerVec=myMesh->bones[i]->boneMatrix->getTranslation(); } float upperLength = initialUpperVec.length(); float lowerLength = initialLowerVec.length(); //initial Full - without any transformations //this is for the relative positions of the bones to each other Vector3f initialFull= initialUpperVec + initialLowerVec; //project upper on full vector to get upper Angle initialUpperVec.normalize(); initialLowerVec.normalize(); initialFull.normalize(); Vector3f currentFull=base->base->baseMatrix.getTranslation() - baseMatrix.getTranslation(); Vector3f targetFull=base->base->baseMatrix.getTranslation() - loc; float targetFullLength = targetFull.length(); ///out of bounds!!! if (targetFullLength>=upperLength+lowerLength){ cout << "IK on " << name << " is out of bounds!" << endl; cout << targetFullLength << endl; return; } currentFull.normalize(); targetFull.normalize(); ///rotating into IK plane Matrix4f parentMatrix=parent->baseMatrix; parentMatrix.setTranslation(Vector3f(0,0,0)); Vector3f rotAxis = currentFull.crossProduct(targetFull); rotAxis=parentMatrix.inverse() * rotAxis; rotAxis.normalize(); float rot= acos(currentFull.dotProduct(targetFull)); //check if they're on each other! if (rot!=rot){ cout << "already in plane!" << endl; rot=0.0; //no rotation in that case! } Vector3f xA,yA,zA; yA=rotAxis; xA=currentFull; zA=yA.crossProduct(xA); zA.normalize(); Matrix4f rotMatrix,angleMatrix; rotMatrix[0]=xA.x; rotMatrix[1]=xA.y; rotMatrix[2]=xA.z; rotMatrix[4]=yA.x; rotMatrix[5]=yA.y; rotMatrix[6]=yA.z; rotMatrix[8]=zA.x; rotMatrix[9]=zA.y; rotMatrix[10]=zA.z; angleMatrix=angleMatrix.createRotationAroundAxis(0,rot,0); rotMatrix=rotMatrix * angleMatrix * rotMatrix.inverse(); //now rotate into IK plane base->base->transformMatrix= base->base->transformMatrix * rotMatrix; //note: we can add the transformMatrix here, since rot should be 0 anyway after this! base->base->baseMatrix=calcMatrix(base->base); base->baseMatrix=calcMatrix(base); baseMatrix=calcMatrix(this); //update all our matrices after transform! ///Math for 2D IK float upperAngle=0.0, middleAngle=0.0, lowerAngle=0.0; //using the following 2D stuff: //sine rule: // a/sinA = b/sinB = c/sinC //cosine rule: //c*c = b*b + a*a + 2ba*cosC //find biggest angle first if (upperLength>lowerLength){ if (upperLength>targetFullLength){ //upperLength = longest! lowerAngle=acos( (lowerLength*lowerLength + targetFullLength*targetFullLength - upperLength*upperLength) / (2* lowerLength * targetFullLength) ); middleAngle=asin( targetFullLength * sin(lowerAngle) / upperLength); upperAngle=asin( lowerLength * sin(lowerAngle)/ upperLength); }else{ //fullLength = longest! middleAngle=acos( (lowerLength*lowerLength + upperLength*upperLength - targetFullLength*targetFullLength) / (2* lowerLength * upperLength) ); lowerAngle=asin( upperLength * sin(middleAngle) / targetFullLength); upperAngle=asin( lowerLength * sin(middleAngle) / targetFullLength); } }else if (lowerLength > targetFullLength){ //lowerLength = longest! upperAngle=acos( ( targetFullLength*targetFullLength + upperLength*upperLength - lowerLength*lowerLength) / (2* upperLength * targetFullLength) ); middleAngle=asin( targetFullLength * sin(lowerAngle) / lowerLength); lowerAngle=asin( upperLength * sin(lowerAngle) / lowerLength); } else{ //fullLength = longest! middleAngle=acos( (lowerLength*lowerLength + upperLength*upperLength - targetFullLength*targetFullLength)/(2* lowerLength * upperLength) ); lowerAngle=asin( upperLength * sin(middleAngle) / targetFullLength); upperAngle=asin( lowerLength * sin(middleAngle) / targetFullLength); } //phew!!! //middle rotation //NaN test! if (middleAngle!=middleAngle) middleAngle=0.0; //test NaN if (upperAngle!=upperAngle) upperAngle=0.0; ///gather current Info currentUpperVec=base->base->baseMatrix.getTranslation() - base->baseMatrix.getTranslation(); currentUpperVec.normalize(); currentLowerVec=base->baseMatrix.getTranslation() - baseMatrix.getTranslation(); currentLowerVec.normalize(); //origUpperAngle is the current 2D rotation! float currentUpperAngle=acos(targetFull.dotProduct(currentUpperVec)); //check for NaN if (currentUpperAngle!=currentUpperAngle) currentUpperAngle=0.0; //project upper on lower Vector to get middle Angle float currentMiddleAngle=acos(currentLowerVec.dotProduct(currentUpperVec)); //check for NaN if (currentMiddleAngle!=currentMiddleAngle) currentMiddleAngle=0.0; //add original values! upperAngle= upperAngle - currentUpperAngle; middleAngle= M_PI + middleAngle + currentMiddleAngle;// -; ///rotate 2D IK Vector3f initialRotYAxis,initialRotZAxis, initialRotXAxis; //now find our rotation Axis for 2D IK! if (currentUpperAngle==0.0){ initialRotYAxis=Vector3f(0,1,0); initialRotZAxis=Vector3f(0,0,1); initialRotXAxis=Vector3f(1,0,0); }else{ initialRotYAxis=initialFull.crossProduct(initialUpperVec); initialRotYAxis=initialUpperVec.crossProduct(initialFull); initialRotYAxis.normalize(); initialRotXAxis=initialUpperVec; initialRotZAxis=initialRotXAxis.crossProduct(initialRotYAxis); initialRotZAxis.normalize(); } xA=initialRotXAxis; yA=initialRotYAxis; zA=initialRotZAxis; Matrix4d initialRotMatrix; initialRotMatrix[0]=xA.x; initialRotMatrix[1]=xA.y; initialRotMatrix[2]=xA.z; initialRotMatrix[4]=yA.x; initialRotMatrix[5]=yA.y; initialRotMatrix[6]=yA.z; initialRotMatrix[8]=zA.x; initialRotMatrix[9]=zA.y; initialRotMatrix[10]=zA.z; //create a rotation matrix that applies the rotations to the x-Axis Matrix4f midMatrix,upMatrix; midMatrix=midMatrix.createRotationAroundAxis(0,middleAngle,0); upMatrix=upMatrix.createRotationAroundAxis(0,upperAngle,0); //rotate around arbitrary axis by rotating to the axis, then rotating your desired rotation, then rotating back midMatrix= initialRotMatrix * midMatrix * initialRotMatrix.inverse(); upMatrix= initialRotMatrix * upMatrix * initialRotMatrix.inverse(); //rotate around first angle first base->base->transformMatrix= base->base->transformMatrix * upMatrix; base->transformMatrix= base->transformMatrix * midMatrix; ///update matrices base->base->baseMatrix=calcMatrix(base->base); base->baseMatrix=calcMatrix(base); baseMatrix=calcMatrix(this); }
void Surface::trans(Matrix4d& m, Matrix4d& inv) { _trans = true; _mInv = inv; _mTrans = m.inverse().transpose(); _b = _b.transform(m); }