//multiply vector by a scalar
cvector scalarvecmul(double complex scalar, cvector input){
int i;
//create output
cvector returnVal = zerovec(input.size);
for(i=0;i<input.size;i++) returnVal.entries[i] = scalar * input.entries[i];
return returnVal;
}
//vector complex conjugate
cvector conjvec(cvector input){
int i;
//create output
cvector returnVal = zerovec(input.size);
for(i=0;i<input.size;i++) returnVal.entries[i] = conj(input.entries[i]);
return returnVal;
}
//add vectors
cvector vecadd(cvector input1, cvector input2){
int i;
//create output
cvector returnVal = zerovec(input1.size);
//fill entries
for(i=0;i<input1.size;i++) returnVal.entries[i] = input1.entries[i] + input2.entries[i];
return returnVal;
}
bool ZernikeMom::CalculateOneFrame(int frame){
const double PI = 3.141592654;
// cout << "ZernikeMom::CalculateOneFrame("<<frame<<")"<<endl;
EnsureImage();
SegmentData()->setCurrentFrame(frame);
TabulateFactorials(param.maxorder);
FindSegmentNormalisations();
int nc=NumberOfCoefficients(param.maxorder);
vector<double> zerovec(nc,0.0);
vector<vector<double> > A_real(DataCount(),zerovec);
vector<vector<double> > A_imag(DataCount(),zerovec);
int width = Width(true), height = Height(true);
int resind=0;
for(int n=0;n<=param.maxorder;n++)
for(int m=n&1;m<=n;m+=2){
for (int y=0; y<height; y++)
for (int x=0; x<width; x++){
vector<int> svec = SegmentVector(frame, x, y);
for (size_t j=0; j<svec.size(); j++) {
int dataind = DataIndex(svec[j], true);
if(dataind != -1){
int label=svec[j];
double xtilde=(x-XAvg(label))*Scaling(label);
double ytilde=(y-YAvg(label))*Scaling(label);
double roo=sqrt(xtilde*xtilde+ytilde*ytilde);
double theta;
if(ytilde || xtilde)
theta=atan2(ytilde,xtilde);
else
theta=0;
double r=R(n,m,roo);
r *= Scaling(label)*Scaling(label)*(n+1)/PI;
A_real[dataind][resind] += r*cos(m*theta);
A_imag[dataind][resind] -= r*sin(m*theta);
}
}
}
resind++;
}
for(size_t i=0;i<A_real.size();i++){
((ZernikeMomData*)GetData(i))->SetData(A_real[i],A_imag[i],param);
}
calculated = true;
return true;
}
void sc_plugin_interface::buffer_zero(uint32_t index)
{
SndBuf * buf = World_GetNRTBuf(&world, index);
uint32_t length = buf->frames * buf->channels;
uint32_t unrolled = length & ~63;
uint32_t remain = length & 63;
zerovec_simd(buf->data, unrolled);
zerovec(buf->data + unrolled, remain);
}
//cross product
cvector cross(cvector leftvector, cvector rightvector){
if(leftvector.size!=3 || rightvector.size!=3){
fprintf(stderr, "Error in function 'cross': Either leftvector size or cols rightvector size is not 3\nPossible reasons:\ncross product of 'leftvector' and 'rightvector' cannot be taken because they must both be of size 3\n");
exit(1);
}
int i;
//create output
cvector returnVal = zerovec(3);
returnVal.entries[0] = leftvector.entries[1]*rightvector.entries[2] - leftvector.entries[2]*rightvector.entries[1];
returnVal.entries[1] = leftvector.entries[2]*rightvector.entries[0] - leftvector.entries[0]*rightvector.entries[2];
returnVal.entries[2] = leftvector.entries[0]*rightvector.entries[1] - leftvector.entries[1]*rightvector.entries[0];
return returnVal;
}
inline void run_scheduler_tick(void)
{
const int blocksize = sc_factory->world.mBufLength;
const int input_channels = sc_factory->world.mNumInputs;
const int output_channels = sc_factory->world.mNumOutputs;
const int buf_counter = ++sc_factory->world.mBufCounter;
/* touch all input buffers */
for (int channel = 0; channel != input_channels; ++channel)
sc_factory->world.mAudioBusTouched[output_channels + channel] = buf_counter;
(*instance)();
/* wipe all untouched output buffers */
for (int channel = 0; channel != output_channels; ++channel) {
if (sc_factory->world.mAudioBusTouched[channel] != buf_counter)
zerovec(sc_factory->world.mAudioBus + blocksize * channel, blocksize);
}
}
/* read input fifo from the rt context */
void read_input_buffers(size_t frames_per_tick)
{
if (reader_running.load(std::memory_order_acquire))
{
const size_t total_samples = input_channels * frames_per_tick;
size_t remaining = total_samples;
read_semaphore.wait();
do {
remaining -= read_frames.pop(temp_buffer.get(), remaining);
if (unlikely(read_frames.empty() &&
!reader_running.load(std::memory_order_acquire)))
{
/* at the end, we are not able to read a full sample block, clear the final parts */
const size_t last_frame = (total_samples - remaining) / input_channels;
const size_t remaining_per_channel = remaining / input_channels;
assert(remaining % input_channels == 0);
assert(remaining_per_channel % input_channels == 0);
for (uint16_t channel = 0; channel != input_channels; ++channel)
zerovec(super::input_samples[channel].get() + last_frame, remaining_per_channel);
break;
}
} while (remaining);
const size_t frames = (total_samples - remaining) / input_channels;
for (size_t frame = 0; frame != frames; ++frame)
{
for (uint16_t channel = 0; channel != input_channels; ++channel)
super::input_samples[channel].get()[frame] = temp_buffer.get()[frame * input_channels + channel];
}
}
else
super::clear_inputs(frames_per_tick);
}
void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
#ifndef __SPU__
if (m_useSolveConstraintObsolete)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btScalar tau = btScalar(0.3);
//linear part
if (!m_angularOnly)
{
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 vel1;
bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2;
bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++)
{
const btVector3& normal = m_jac[i].m_linearJointAxis;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
btScalar rel_vel;
rel_vel = normal.dot(vel);
//positional error (zeroth order error)
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
m_appliedImpulse += impulse;
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
}
}
// apply motor
if (m_bMotorEnabled)
{
// compute current and predicted transforms
btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform();
btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
btTransform trAPred; trAPred.setIdentity();
btVector3 zerovec(0,0,0);
btTransformUtil::integrateTransform(
trACur, zerovec, omegaA, timeStep, trAPred);
btTransform trBPred; trBPred.setIdentity();
btTransformUtil::integrateTransform(
trBCur, zerovec, omegaB, timeStep, trBPred);
// compute desired transforms in world
btTransform trPose(m_qTarget);
btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
btTransform trADes = trBPred * trABDes;
btTransform trBDes = trAPred * trABDes.inverse();
// compute desired omegas in world
btVector3 omegaADes, omegaBDes;
btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
// compute delta omegas
btVector3 dOmegaA = omegaADes - omegaA;
btVector3 dOmegaB = omegaBDes - omegaB;
// compute weighted avg axis of dOmega (weighting based on inertias)
btVector3 axisA, axisB;
btScalar kAxisAInv = 0, kAxisBInv = 0;
if (dOmegaA.length2() > SIMD_EPSILON)
{
axisA = dOmegaA.normalized();
kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
}
if (dOmegaB.length2() > SIMD_EPSILON)
{
axisB = dOmegaB.normalized();
kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
}
btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
static bool bDoTorque = true;
if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
{
avgAxis.normalize();
kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
btScalar kInvCombined = kAxisAInv + kAxisBInv;
btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
(kInvCombined * kInvCombined);
if (m_maxMotorImpulse >= 0)
{
btScalar fMaxImpulse = m_maxMotorImpulse;
if (m_bNormalizedMotorStrength)
fMaxImpulse = fMaxImpulse/kAxisAInv;
btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
btScalar newUnclampedMag = newUnclampedAccImpulse.length();
if (newUnclampedMag > fMaxImpulse)
{
newUnclampedAccImpulse.normalize();
newUnclampedAccImpulse *= fMaxImpulse;
impulse = newUnclampedAccImpulse - m_accMotorImpulse;
}
m_accMotorImpulse += impulse;
}
btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
}
}
else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
{
btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
btVector3 relVel = angVelB - angVelA;
if (relVel.length2() > SIMD_EPSILON)
{
btVector3 relVelAxis = relVel.normalized();
btScalar m_kDamping = btScalar(1.) /
(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
btVector3 impulse = m_damping * m_kDamping * relVel;
btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag;
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
}
}
// joint limits
{
///solve angular part
btVector3 angVelA;
bodyA.internalGetAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.internalGetAngularVelocity(angVelB);
// solve swing limit
if (m_solveSwingLimit)
{
btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
if (relSwingVel > 0)
amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
btScalar impulseMag = amplitude * m_kSwing;
// Clamp the accumulated impulse
btScalar temp = m_accSwingLimitImpulse;
m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accSwingLimitImpulse - temp;
btVector3 impulse = m_swingAxis * impulseMag;
// don't let cone response affect twist
// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
{
btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
impulse = impulseNoTwistCouple;
}
impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
}
// solve twist limit
if (m_solveTwistLimit)
{
btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
btScalar impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
btScalar temp = m_accTwistLimitImpulse;
m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accTwistLimitImpulse - temp;
// btVector3 impulse = m_twistAxis * impulseMag;
bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
}
}
}
#else
btAssert(0);
#endif //__SPU__
}
//multiply a vector by a matrix
cvector matmulvec(cmatrix muliplier, cvector input){
cvector returnVal = zerovec(input.size);
for(int i=0;i<input.size;i++) for(int j=0;j<input.size;j++) returnVal.entries[i] += muliplier.entries[i][j]*input.entries[j];
return returnVal;
}
void blochsim(double *b1real, double *b1imag,
double *xgrad, double *ygrad, double *zgrad, double *tsteps,
int ntime, double *e1, double *e2, double df,
double dx, double dy, double dz,
double *mx, double *my, double *mz, int mode)
/* Go through time for one df and one dx,dy,dz. */
{
int tcount;
double gammadx;
double gammady;
double gammadz;
double rotmat[9];
double amat[9], bvec[3]; /* A and B propagation matrix and vector */
double arot[9], brot[3]; /* A and B after rotation step. */
double decmat[9]; /* Decay matrix for each time step. */
double decvec[3]; /* Recovery vector for each time step. */
double rotx,roty,rotz; /* Rotation axis coordinates. */
double imat[9], mvec[3];
double mcurr0[3]; /* Current magnetization before rotation. */
double mcurr1[3]; /* Current magnetization before decay. */
eyemat(amat); /* A is the identity matrix. */
eyemat(imat); /* I is the identity matrix. */
zerovec(bvec);
zerovec(decvec);
zeromat(decmat);
gammadx = dx*GAMMA; /* Convert to Hz/cm */
gammady = dy*GAMMA; /* Convert to Hz/cm */
gammadz = dz*GAMMA; /* Convert to Hz/cm */
mcurr0[0] = *mx; /* Set starting x magnetization */
mcurr0[1] = *my; /* Set starting y magnetization */
mcurr0[2] = *mz; /* Set starting z magnetization */
for (tcount = 0; tcount < ntime; tcount++)
{
/* Rotation */
rotz = -(*xgrad++ * gammadx + *ygrad++ * gammady + *zgrad++ * gammadz +
df*TWOPI ) * *tsteps;
rotx = (- *b1real++ * GAMMA * *tsteps);
roty = (+ *b1imag++ * GAMMA * *tsteps++);
calcrotmat(rotx, roty, rotz, rotmat);
if (mode == 1)
{
multmats(rotmat,amat,arot);
multmatvec(rotmat,bvec,brot);
}
else
multmatvec(rotmat,mcurr0,mcurr1);
/* Decay */
decvec[2]= 1- *e1;
decmat[0]= *e2;
decmat[4]= *e2++;
decmat[8]= *e1++;
if (mode == 1)
{
multmats(decmat,arot,amat);
multmatvec(decmat,brot,bvec);
addvecs(bvec,decvec,bvec);
}
else
{
multmatvec(decmat,mcurr1,mcurr0);
addvecs(mcurr0,decvec,mcurr0);
}
/*
printf("rotmat = [%6.3f %6.3f %6.3f ] \n",rotmat[0],rotmat[3],
rotmat[6]);
printf(" [%6.3f %6.3f %6.3f ] \n",rotmat[1],rotmat[4],
rotmat[7]);
printf(" [%6.3f %6.3f %6.3f ] \n",rotmat[2],rotmat[5],
rotmat[8]);
printf("A = [%6.3f %6.3f %6.3f ] \n",amat[0],amat[3],amat[6]);
printf(" [%6.3f %6.3f %6.3f ] \n",amat[1],amat[4],amat[7]);
printf(" [%6.3f %6.3f %6.3f ] \n",amat[2],amat[5],amat[8]);
printf(" B = <%6.3f,%6.3f,%6.3f> \n",bvec[0],bvec[1],bvec[2]);
printf("<mx,my,mz> = <%6.3f,%6.3f,%6.3f> \n",
amat[6] + bvec[0], amat[7] + bvec[1], amat[8] + bvec[2]);
printf("\n");
*/
if (mode == 2) /* Sample output at times. */
/* Only do this if transient! */
{
*mx = mcurr0[0];
*my = mcurr0[1];
*mz = mcurr0[2];
mx++;
my++;
mz++;
}
}
/* If only recording the endpoint, either store the last
point, or calculate the steady-state endpoint. */
if (mode==0) /* Indicates start at given m, or m0. */
{
*mx = mcurr0[0];
*my = mcurr0[1];
*mz = mcurr0[2];
}
else if (mode==1) /* Indicates to find steady-state magnetization */
{
scalemat(amat,-1.0); /* Negate A matrix */
addmats(amat,imat,amat); /* Now amat = (I-A) */
invmat(amat,imat); /* Reuse imat as inv(I-A) */
multmatvec(imat,bvec,mvec); /* Now M = inv(I-A)*B */
*mx = mvec[0];
*my = mvec[1];
*mz = mvec[2];
}
}
