static int PM_CheckJump() {
pmove_t *xm = *(pmove_t**)(int)pm;
#if 0
if(xm->cmd.wbuttons & WBUTTON_RELOAD) {
return 0;
}
#endif
int (*cj)() = (int(*)())GAME("BG_PlayerTouchesItem") + 0x7DC;
return cj();
}
Foam::autoPtr<Foam::RBD::joints::composite>
Foam::RBD::joints::floating::sixDoF()
{
PtrList<joint> cj(2);
cj.set(0, new joints::Pxyz());
// The quaternion-based spherical joint could be used
// but then w must be set appropriately
//cj.set(1, new joints::Rs());
// Alternatively the Euler-angle joint can be used
cj.set(1, new joints::Rzyx());
return autoPtr<composite>(new composite(cj));
}
//==============================================================
// Checks events of predicted collision partner to keep collisions
// symmetric
//==============================================================
void box::CollisionChecker(event c)
{
int i = c.i;
int j = c.j;
event cj(c.time,j,i,c.v*(-1));
// j should have NO event before collision with i!
if (!(c.time < s[j].nextevent.time))
std::cout << i << " " << j << " error collchecker, s[j].nextevent.time= " << s[j].nextevent.time << " " << s[j].nextevent.j << ", c.time= " << c.time << std::endl;
int k = s[j].nextevent.j;
if ((k < N) && (k!=i)) // j's next event was collision so give k a check
s[k].nextevent.j = INF;
// give collision cj to j
s[j].nextevent = cj;
h.upheap(h.index[j]);
}
void CalculateAngleDerivativesE2(vtkPolyData* mesh, const arma::sp_mat& edge_lengths, const arma::vec& radii) {
//calculate the power center for each triangle as follows
//for 2 circles centered at c1 and c2 solve for p:
//|p-c1|^2 - r1^2 = |p-c2|^2 - r2^2 -- this is power line
//add one more condition for the third circle to get one point
// Funny part: the paper says that the circle of radius sqrt(|c1-p|^2 - r1^2)
// is perpendicular to all three circles. But this works only when the 3 circles do not intersect
const int npts = mesh->GetNumberOfPoints();
const int ncells = mesh->GetNumberOfCells();
vtkSmartPointer<vtkIdList> cellIdList = vtkSmartPointer<vtkIdList>::New();
vtkSmartPointer<vtkIdList> pointIdList = vtkSmartPointer<vtkIdList>::New();
//for every face calculate gamma_ijk
arma::vec gamma_percell(ncells);
for (vtkIdType i = 0; i < ncells; i++) {
pointIdList->Reset();
mesh->GetCellPoints(cellIdList->GetId(i), pointIdList);
const int ind_i = pointIdList->GetId(0);
const int ind_j = pointIdList->GetId(1);
const int ind_k = pointIdList->GetId(2);
const double ri = radii[ind_i];
const double rj = radii[ind_j];
const double rk = radii[ind_k];
arma::vec ci(3), cj(3), ck(3);
mesh->GetPoint(ind_i, ci.memptr());
mesh->GetPoint(ind_j, cj.memptr());
mesh->GetPoint(ind_k, ck.memptr());
//create local 2D coordinates, and map vertices
arma::vec vji = cj - ci;
arma::vec v = ck - ci;
arma::vec n = arma::cross(vji, v); //normal
arma::vec vki = arma::cross(n, vji); //creates local y' axis
const double lenX = arma::norm(vji,2);
const double lenY = arma::norm(vki,2);
vji /= lenX; //normalize axes
vki /= lenY;
//2D coordinates
arma::vec vi(3,0); //(0,0)
arma::vec vj(3);
arma::vec vk(3);
vj(0) = lenX;
vj(1) = 0.0;
vk(0) = arma::dot(v, vji);
vk(1) = arma::dot(v, vki);
//form matrices for system of linear equations and solve
//A = [2*(c2-c1)'; 2*(c2-c3)'];
//B = [ c2'*c2 - c1'*c1 + r1^2 - r2^2; c2'*c2 - c3'*c3 + r3^2 - r2^2];
//P = A\B;
arma::mat A(2,2);
arma::vec B(2);
A.insert_rows( 0, 2.0*(cj-ci) );
A.insert_rows( 1, 2.0*(cj-ck) );
B(0) = arma::dot(cj, cj) - arma::dot(ci, ci) + ri*ri - rj*rj;
B(1) = arma::dot(cj, cj) - arma::dot(ck, ck) + rk*rk - rj*rj;
arma::vec P = arma::solve( A, B );
//calculate distance from P to all the edges: hi, hj, hk
//http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
const double hi = arma::norm( arma::cross(cj-ck, ck-P), 2 ) / arma::norm(cj-ck, 2);
const double hj = arma::norm( arma::cross(ci-ck, ck-P), 2 ) / arma::norm(ci-ck, 2);
const double hk = arma::norm( arma::cross(cj-ci, ci-P), 2 ) / arma::norm(cj-ci, 2);
const double li = edge_lengths(ind_j, ind_k);
const double lj = edge_lengths(ind_i, ind_k);
const double lk = edge_lengths(ind_i, ind_j);
const double dTi_duj = hk/lk;
const double dTj_duk = hi/li;
const double dTk_dui = hj/lj;
const double dTi_dui = - dTi_duj - dTk_dui;
const double dTj_duj = - dTj_duk - dTi_duj;
const double dTk_duk = - dTj_duk - dTk_dui;
}
}
template <class P> static
void Apply( const GenericImage<P>& image, MultiscaleMedianTransform& T )
{
InitializeStructures();
bool statusInitialized = false;
StatusMonitor& status = (StatusMonitor&)image.Status();
try
{
if ( status.IsInitializationEnabled() )
{
status.Initialize( String( T.m_medianWaveletTransform ? "Median-wavelet" : "Multiscale median" ) + " transform",
image.NumberOfSelectedSamples()*T.m_numberOfLayers*(T.m_medianWaveletTransform ? 2 : 1) );
status.DisableInitialization();
statusInitialized = true;
}
GenericImage<P> cj0( image );
cj0.Status().Clear();
for ( int j = 1, j0 = 0; ; ++j, ++j0 )
{
GenericImage<P> cj( cj0 );
cj.Status() = status;
MedianFilterLayer( cj, T.FilterSize( j0 ), T.m_parallel, T.m_maxProcessors );
if ( T.m_medianWaveletTransform )
{
GenericImage<P> w0( cj0 );
GenericImage<P> d0( cj0 );
d0 -= cj;
for ( int c = 0; c < d0.NumberOfChannels(); ++c )
{
w0.SelectChannel( c );
d0.SelectChannel( c );
cj.SelectChannel( c );
double t = T.m_medianWaveletThreshold*d0.MAD( d0.Median() )/0.6745;
for ( typename GenericImage<P>::sample_iterator iw( w0 ), id( d0 ), ic( cj ); iw; ++iw, ++id, ++ic )
if ( Abs( *id ) > t )
*iw = *ic;
}
w0.ResetSelections();
cj.ResetSelections();
w0.Status() = cj.Status();
LinearFilterLayer( w0, T.FilterSize( j0 ), T.m_parallel, T.m_maxProcessors );
cj = w0;
}
status = cj.Status();
cj.Status().Clear();
if ( T.m_layerEnabled[j0] )
{
cj0 -= cj;
T.m_transform[j0] = Image( cj0 );
}
if ( j == T.m_numberOfLayers )
{
if ( T.m_layerEnabled[j] )
T.m_transform[j] = Image( cj );
break;
}
cj0 = cj;
}
if ( statusInitialized )
status.EnableInitialization();
}
catch ( ... )
{
T.DestroyLayers();
if ( statusInitialized )
status.EnableInitialization();
throw;
}
}
void TCPSrc::feed(SampleVector::const_iterator begin, SampleVector::const_iterator end, bool positiveOnly)
{
Complex ci;
cmplx* sideband;
Real l, r;
m_sampleBuffer.clear();
// Rtl-Sdr uses full 16-bit scale; FCDPP does not
int rescale = 30000 * (1 << m_boost);
for(SampleVector::const_iterator it = begin; it < end; ++it) {
Complex c(it->real() / 32768.0, it->imag() / 32768.0);
c *= m_nco.nextIQ();
if(m_interpolator.interpolate(&m_sampleDistanceRemain, c, &ci)) {
m_sampleBuffer.push_back(Sample(ci.real() * rescale, ci.imag() * rescale));
m_sampleDistanceRemain += m_inputSampleRate / m_outputSampleRate;
}
}
if((m_spectrum != NULL) && (m_spectrumEnabled))
m_spectrum->feed(m_sampleBuffer.begin(), m_sampleBuffer.end(), positiveOnly);
for(int i = 0; i < m_s16leSockets.count(); i++)
m_s16leSockets[i].socket->write((const char*)&m_sampleBuffer[0], m_sampleBuffer.size() * 4);
if((m_sampleFormat == FormatSSB) && (m_ssbSockets.count() > 0)) {
for(SampleVector::const_iterator it = m_sampleBuffer.begin(); it != m_sampleBuffer.end(); ++it) {
Complex cj(it->real() / 30000.0, it->imag() / 30000.0);
int n_out = TCPFilter->runSSB(cj, &sideband, true);
if (n_out) {
for (int i = 0; i < n_out; i+=2) {
l = (sideband[i].real() + sideband[i].imag()) * 0.7 * 32000.0;
r = (sideband[i+1].real() + sideband[i+1].imag()) * 0.7 * 32000.0;
m_sampleBufferSSB.push_back(Sample(l, r));
}
for(int i = 0; i < m_ssbSockets.count(); i++)
m_ssbSockets[i].socket->write((const char*)&m_sampleBufferSSB[0], n_out * 2);
m_sampleBufferSSB.clear();
}
}
}
if((m_sampleFormat == FormatNFM) && (m_ssbSockets.count() > 0)) {
for(SampleVector::const_iterator it = m_sampleBuffer.begin(); it != m_sampleBuffer.end(); ++it) {
Complex cj(it->real() / 30000.0, it->imag() / 30000.0);
// An FFT filter here is overkill, but was already set up for SSB
int n_out = TCPFilter->runFilt(cj, &sideband);
if (n_out) {
Real sum = 1.0;
for (int i = 0; i < n_out; i+=2) {
l = m_this.real() * (m_last.imag() - sideband[i].imag())
- m_this.imag() * (m_last.real() - sideband[i].real());
m_last = sideband[i];
r = m_last.real() * (m_this.imag() - sideband[i+1].imag())
- m_last.imag() * (m_this.real() - sideband[i+1].real());
m_this = sideband[i+1];
m_sampleBufferSSB.push_back(Sample(l * m_scale, r * m_scale));
sum += m_this.real() * m_this.real() + m_this.imag() * m_this.imag();
}
// TODO: correct levels
m_scale = 24000 * tcpFftLen / sum;
for(int i = 0; i < m_ssbSockets.count(); i++)
m_ssbSockets[i].socket->write((const char*)&m_sampleBufferSSB[0], n_out * 2);
m_sampleBufferSSB.clear();
}
}
}
}