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);
}
