static void hhldr3rows(MAT *A, int k, int i0, double beta,
double nu1, double nu2, double nu3)
#endif
{
Real **A_me, ip, prod;
int i, m;
/* printf("hhldr3rows:(l.%d) A at 0x%lx\n", __LINE__, (long)A); */
/* printf("hhldr3rows: k = %d\n", k); */
if ( k < 0 || k+3 > A->n )
error(E_BOUNDS,"hhldr3rows");
A_me = A->me; m = A->m;
i0 = min(i0,m-1);
for ( i = 0; i <= i0; i++ )
{
/****
ip = nu1*A_me[i][k] + nu2*A_me[i][k+1] + nu3*A_me[i][k+2];
prod = ip*beta;
A_me[i][k] -= prod*nu1;
A_me[i][k+1] -= prod*nu2;
A_me[i][k+2] -= prod*nu3;
****/
ip = nu1*m_entry(A,i,k)+nu2*m_entry(A,i,k+1)+nu3*m_entry(A,i,k+2);
prod = ip*beta;
m_add_val(A,i,k , - prod*nu1);
m_add_val(A,i,k+1, - prod*nu2);
m_add_val(A,i,k+2, - prod*nu3);
}
}
static void hhldr3cols(MAT *A, int k, int j0, double beta,
double nu1, double nu2, double nu3)
#endif
{
Real **A_me, ip, prod;
int j, n;
if ( k < 0 || k+3 > A->m || j0 < 0 )
error(E_BOUNDS,"hhldr3cols");
A_me = A->me; n = A->n;
/* printf("hhldr3cols:(l.%d) j0 = %d, k = %d, A at 0x%lx, m = %d, n = %d\n",
__LINE__, j0, k, (long)A, A->m, A->n); */
/* printf("hhldr3cols: A (dumped) =\n"); m_dump(stdout,A); */
for ( j = j0; j < n; j++ )
{
/*****
ip = nu1*A_me[k][j] + nu2*A_me[k+1][j] + nu3*A_me[k+2][j];
prod = ip*beta;
A_me[k][j] -= prod*nu1;
A_me[k+1][j] -= prod*nu2;
A_me[k+2][j] -= prod*nu3;
*****/
/* printf("hhldr3cols: j = %d\n", j); */
ip = nu1*m_entry(A,k,j)+nu2*m_entry(A,k+1,j)+nu3*m_entry(A,k+2,j);
prod = ip*beta;
/*****
m_set_val(A,k ,j,m_entry(A,k ,j) - prod*nu1);
m_set_val(A,k+1,j,m_entry(A,k+1,j) - prod*nu2);
m_set_val(A,k+2,j,m_entry(A,k+2,j) - prod*nu3);
*****/
m_add_val(A,k ,j,-prod*nu1);
m_add_val(A,k+1,j,-prod*nu2);
m_add_val(A,k+2,j,-prod*nu3);
}
/* printf("hhldr3cols:(l.%d) j0 = %d, k = %d, m = %d, n = %d\n",
__LINE__, j0, k, A->m, A->n); */
/* putc('\n',stdout); */
}
/* rot_rows -- premultiply mat by givens rotation described by c,s */
extern MAT *rot_rows(MAT *mat,u_int i, u_int k, double c, double s, MAT *out)
{
u_int j;
Real temp;
if ( mat==(MAT *)NULL )
error(E_NULL,"rot_rows");
if ( i >= mat->m || k >= mat->m )
error(E_RANGE,"rot_rows");
out = m_copy(mat,out);
for ( j=0; j<mat->n; j++ )
{
/* temp = c*out->me[i][j] + s*out->me[k][j]; */
temp = c*m_entry(out,i,j) + s*m_entry(out,k,j);
/* out->me[k][j] = -s*out->me[i][j] + c*out->me[k][j]; */
m_set_val(out,k,j, -s*m_entry(out,i,j) + c*m_entry(out,k,j));
/* out->me[i][j] = temp; */
m_set_val(out,i,j, temp);
}
return (out);
}
/* rot_cols -- postmultiply mat by givens rotation described by c,s */
extern MAT *rot_cols(MAT *mat,u_int i, u_int k, double c, double s, MAT *out)
{
u_int j;
Real temp;
if ( mat==(MAT *)NULL )
error(E_NULL,"rot_cols");
if ( i >= mat->n || k >= mat->n )
error(E_RANGE,"rot_cols");
out = m_copy(mat,out);
for ( j=0; j<mat->m; j++ )
{
/* temp = c*out->me[j][i] + s*out->me[j][k]; */
temp = c*m_entry(out,j,i) + s*m_entry(out,j,k);
/* out->me[j][k] = -s*out->me[j][i] + c*out->me[j][k]; */
m_set_val(out,j,k, -s*m_entry(out,j,i) + c*m_entry(out,j,k));
/* out->me[j][i] = temp; */
m_set_val(out,j,i,temp);
}
return (out);
}
/* interchange -- a row/column swap routine */
static void interchange(MAT *A, int i, int j)
/* assumed != NULL & also SQUARE */
/* assumed in range */
{
Real **A_me, tmp;
int k, n;
A_me = A->me; n = A->n;
if ( i == j )
return;
if ( i > j )
{ k = i; i = j; j = k; }
for ( k = 0; k < i; k++ )
{
/* tmp = A_me[k][i]; */
tmp = m_entry(A,k,i);
/* A_me[k][i] = A_me[k][j]; */
m_set_val(A,k,i,m_entry(A,k,j));
/* A_me[k][j] = tmp; */
m_set_val(A,k,j,tmp);
}
for ( k = j+1; k < n; k++ )
{
/* tmp = A_me[j][k]; */
tmp = m_entry(A,j,k);
/* A_me[j][k] = A_me[i][k]; */
m_set_val(A,j,k,m_entry(A,i,k));
/* A_me[i][k] = tmp; */
m_set_val(A,i,k,tmp);
}
for ( k = i+1; k < j; k++ )
{
/* tmp = A_me[k][j]; */
tmp = m_entry(A,k,j);
/* A_me[k][j] = A_me[i][k]; */
m_set_val(A,k,j,m_entry(A,i,k));
/* A_me[i][k] = tmp; */
m_set_val(A,i,k,tmp);
}
/* tmp = A_me[i][i]; */
tmp = m_entry(A,i,i);
/* A_me[i][i] = A_me[j][j]; */
m_set_val(A,i,i,m_entry(A,j,j));
/* A_me[j][j] = tmp; */
m_set_val(A,j,j,tmp);
}
/* BKPsolve -- solves A.x = b where A has been factored a la BKPfactor()
-- returns x, which is created if NULL */
extern VEC *BKPsolve(MAT *A, PERM *pivot, PERM *block, VEC *b, VEC *x)
{
static VEC *tmp=VNULL; /* dummy storage needed */
int i, j, n, onebyone;
Real **A_me, a11, a12, a22, b1, b2, det, sum, *tmp_ve, tmp_diag;
if ( ! A || ! pivot || ! block || ! b )
error(E_NULL,"BKPsolve");
if ( A->m != A->n )
error(E_SQUARE,"BKPsolve");
n = A->n;
if ( b->dim != n || pivot->size != n || block->size != n )
error(E_SIZES,"BKPsolve");
x = v_resize(x,n);
tmp = v_resize(tmp,n);
MEM_STAT_REG(tmp,TYPE_VEC);
A_me = A->me; tmp_ve = tmp->ve;
px_vec(pivot,b,tmp);
/* solve for lower triangular part */
for ( i = 0; i < n; i++ )
{
sum = v_entry(tmp,i);
if ( block->pe[i] < i )
for ( j = 0; j < i-1; j++ )
sum -= m_entry(A,i,j)*v_entry(tmp,j);
else
for ( j = 0; j < i; j++ )
sum -= m_entry(A,i,j)*v_entry(tmp,j);
v_set_val(tmp,i,sum);
}
/* printf("# BKPsolve: solving L part: tmp =\n"); v_output(tmp); */
/* solve for diagonal part */
for ( i = 0; i < n; i = onebyone ? i+1 : i+2 )
{
onebyone = ( block->pe[i] == i );
if ( onebyone )
{
tmp_diag = m_entry(A,i,i);
if ( tmp_diag == 0.0 )
error(E_SING,"BKPsolve");
/* tmp_ve[i] /= tmp_diag; */
v_set_val(tmp,i,v_entry(tmp,i) / tmp_diag);
}
else
{
a11 = m_entry(A,i,i);
a22 = m_entry(A,i+1,i+1);
a12 = m_entry(A,i+1,i);
b1 = v_entry(tmp,i); b2 = v_entry(tmp,i+1);
det = a11*a22-a12*a12; /* < 0 : see BKPfactor() */
if ( det == 0.0 )
error(E_SING,"BKPsolve");
det = 1/det;
v_set_val(tmp,i,det*(a22*b1-a12*b2));
v_set_val(tmp,i+1,det*(a11*b2-a12*b1));
}
}
/* printf("# BKPsolve: solving D part: tmp =\n"); v_output(tmp); */
/* solve for transpose of lower traingular part */
for ( i = n-1; i >= 0; i-- )
{ /* use symmetry of factored form to get stride 1 */
sum = v_entry(tmp,i);
if ( block->pe[i] > i )
for ( j = i+2; j < n; j++ )
sum -= m_entry(A,i,j)*v_entry(tmp,j);
else
for ( j = i+1; j < n; j++ )
sum -= m_entry(A,i,j)*v_entry(tmp,j);
v_set_val(tmp,i,sum);
}
/* printf("# BKPsolve: solving L^T part: tmp =\n");v_output(tmp); */
/* and do final permutation */
x = pxinv_vec(pivot,tmp,x);
return x;
}
/* BKPfactor -- Bunch-Kaufman-Parlett factorisation of A in-situ
-- A is factored into the form P'AP = MDM' where
P is a permutation matrix, M lower triangular and D is block
diagonal with blocks of size 1 or 2
-- P is stored in pivot; blocks[i]==i iff D[i][i] is a block */
extern MAT *BKPfactor(MAT *A, PERM *pivot, PERM *blocks)
{
int i, j, k, n, onebyone, r;
Real **A_me, aii, aip1, aip1i, lambda, sigma, tmp;
Real det, s, t;
if ( ! A || ! pivot || ! blocks )
error(E_NULL,"BKPfactor");
if ( A->m != A->n )
error(E_SQUARE,"BKPfactor");
if ( A->m != pivot->size || pivot->size != blocks->size )
error(E_SIZES,"BKPfactor");
n = A->n;
A_me = A->me;
px_ident(pivot); px_ident(blocks);
for ( i = 0; i < n; i = onebyone ? i+1 : i+2 )
{
/* printf("# Stage: %d\n",i); */
aii = fabs(m_entry(A,i,i));
lambda = 0.0; r = (i+1 < n) ? i+1 : i;
for ( k = i+1; k < n; k++ )
{
tmp = fabs(m_entry(A,i,k));
if ( tmp >= lambda )
{
lambda = tmp;
r = k;
}
}
/* printf("# lambda = %g, r = %d\n", lambda, r); */
/* printf("# |A[%d][%d]| = %g\n",r,r,fabs(m_entry(A,r,r))); */
/* determine if 1x1 or 2x2 block, and do pivoting if needed */
if ( aii >= alpha*lambda )
{
onebyone = TRUE;
goto dopivot;
}
/* compute sigma */
sigma = 0.0;
for ( k = i; k < n; k++ )
{
if ( k == r )
continue;
tmp = ( k > r ) ? fabs(m_entry(A,r,k)) :
fabs(m_entry(A,k,r));
if ( tmp > sigma )
sigma = tmp;
}
if ( aii*sigma >= alpha*sqr(lambda) )
onebyone = TRUE;
else if ( fabs(m_entry(A,r,r)) >= alpha*sigma )
{
/* printf("# Swapping rows/cols %d and %d\n",i,r); */
interchange(A,i,r);
px_transp(pivot,i,r);
onebyone = TRUE;
}
else
{
/* printf("# Swapping rows/cols %d and %d\n",i+1,r); */
interchange(A,i+1,r);
px_transp(pivot,i+1,r);
px_transp(blocks,i,i+1);
onebyone = FALSE;
}
/* printf("onebyone = %s\n",btos(onebyone)); */
/* printf("# Matrix so far (@checkpoint A) =\n"); */
/* m_output(A); */
/* printf("# pivot =\n"); px_output(pivot); */
/* printf("# blocks =\n"); px_output(blocks); */
dopivot:
if ( onebyone )
{ /* do one by one block */
if ( m_entry(A,i,i) != 0.0 )
{
aii = m_entry(A,i,i);
for ( j = i+1; j < n; j++ )
{
tmp = m_entry(A,i,j)/aii;
for ( k = j; k < n; k++ )
m_sub_val(A,j,k,tmp*m_entry(A,i,k));
m_set_val(A,i,j,tmp);
}
}
}
else /* onebyone == FALSE */
{ /* do two by two block */
det = m_entry(A,i,i)*m_entry(A,i+1,i+1)-sqr(m_entry(A,i,i+1));
/* Must have det < 0 */
/* printf("# det = %g\n",det); */
aip1i = m_entry(A,i,i+1)/det;
aii = m_entry(A,i,i)/det;
aip1 = m_entry(A,i+1,i+1)/det;
for ( j = i+2; j < n; j++ )
{
s = - aip1i*m_entry(A,i+1,j) + aip1*m_entry(A,i,j);
t = - aip1i*m_entry(A,i,j) + aii*m_entry(A,i+1,j);
for ( k = j; k < n; k++ )
m_sub_val(A,j,k,m_entry(A,i,k)*s + m_entry(A,i+1,k)*t);
m_set_val(A,i,j,s);
m_set_val(A,i+1,j,t);
}
}
/* printf("# Matrix so far (@checkpoint B) =\n"); */
/* m_output(A); */
/* printf("# pivot =\n"); px_output(pivot); */
/* printf("# blocks =\n"); px_output(blocks); */
}
/* set lower triangular half */
for ( i = 0; i < A->m; i++ )
for ( j = 0; j < i; j++ )
m_set_val(A,i,j,m_entry(A,j,i));
return A;
}
/*!
Transforms Symbian device name, address and service classes to QBluetootDeviceInfo.
*/
QBluetoothDeviceInfo BluetoothLinkManagerDeviceDiscoverer::currentDeviceDataToQBluetoothDeviceInfo() const
{
// extract device information from results and map them to QBluetoothDeviceInfo
#ifdef EIR_SUPPORTED
TBluetoothNameRecordWrapper eir(m_entry());
TInt bufferlength = 0;
TInt err = KErrNone;
QString deviceName;
bufferlength = eir.GetDeviceNameLength();
if (bufferlength > 0) {
TBool nameComplete;
HBufC *deviceNameBuffer = 0;
TRAP(err,deviceNameBuffer = HBufC::NewLC(bufferlength));
if(err)
deviceName = QString();
else
{
TPtr ptr = deviceNameBuffer->Des();
err = eir.GetDeviceName(ptr,nameComplete);
if (err == KErrNone /*&& nameComplete*/)
{
if(!nameComplete)
qWarning() << "device name incomplete";
// isn't it better to get an incomplete name than getting nothing?
deviceName = QString::fromUtf16(ptr.Ptr(), ptr.Length()).toUpper();
}
else
deviceName = QString();
CleanupStack::PopAndDestroy(deviceNameBuffer);
}
}
QList<QBluetoothUuid> serviceUidList;
RExtendedInquiryResponseUUIDContainer uuidContainer;
QBluetoothDeviceInfo::DataCompleteness completenes = QBluetoothDeviceInfo::DataUnavailable;
if (eir.GetServiceClassUuids(uuidContainer) == KErrNone) {
TInt uuidCount = uuidContainer.UUIDs().Count();
if (uuidCount > 0) {
for (int i = 0; i < uuidCount; ++i) {
TPtrC8 shortFormUUid(uuidContainer.UUIDs()[i].ShortestForm());
if (shortFormUUid.Size() == 2) {
QBluetoothUuid uuid(ntohs(*reinterpret_cast<const quint16 *>(shortFormUUid.Ptr())));
if (uuidContainer.GetCompleteness(RExtendedInquiryResponseUUIDContainer::EUUID16))
completenes = QBluetoothDeviceInfo::DataComplete;
else
completenes = QBluetoothDeviceInfo::DataIncomplete;
serviceUidList.append(uuid);
}else if (shortFormUUid.Size() == 4) {
QBluetoothUuid uuid(ntohl(*reinterpret_cast<const quint32 *>(shortFormUUid.Ptr())));
if (uuidContainer.GetCompleteness(RExtendedInquiryResponseUUIDContainer::EUUID32))
completenes = QBluetoothDeviceInfo::DataComplete;
else
completenes = QBluetoothDeviceInfo::DataIncomplete;
serviceUidList.append(uuid);
}else if (shortFormUUid.Size() == 16) {
QBluetoothUuid uuid(*reinterpret_cast<const quint128 *>(shortFormUUid.Ptr()));
if (uuidContainer.GetCompleteness(RExtendedInquiryResponseUUIDContainer::EUUID128))
completenes = QBluetoothDeviceInfo::DataComplete;
else
completenes = QBluetoothDeviceInfo::DataIncomplete;
serviceUidList.append(uuid);
}
}
}
}
uuidContainer.Close();
bufferlength = 0;
QByteArray manufacturerData;
bufferlength = eir.GetVendorSpecificDataLength();
if (bufferlength > 0) {
HBufC8 *msd = 0;
TRAP(err,HBufC8::NewLC(bufferlength));
if(err)
manufacturerData = QByteArray();
else
{
TPtr8 temp = msd->Des();
if (eir.GetVendorSpecificData(temp))
manufacturerData = s60Desc8ToQByteArray(temp);
else
manufacturerData = QByteArray();
CleanupStack::PopAndDestroy(msd);
}
}
// Get transmission power level
TInt8 transmissionPowerLevel = 0;
eir.GetTxPowerLevel(transmissionPowerLevel);
// unique address of the device
const TBTDevAddr symbianDeviceAddress = static_cast<TBTSockAddr> (m_entry().iAddr).BTAddr();
QBluetoothAddress bluetoothAddress = qTBTDevAddrToQBluetoothAddress(symbianDeviceAddress);
// format symbian major/minor numbers
quint32 deviceClass = qTPackSymbianDeviceClass(m_addr);
QBluetoothDeviceInfo deviceInfo(bluetoothAddress, deviceName, deviceClass);
deviceInfo.setRssi(transmissionPowerLevel);
deviceInfo.setServiceUuids(serviceUidList, completenes);
deviceInfo.setManufacturerSpecificData(manufacturerData);
#else
// device name
THostName symbianDeviceName = m_entry().iName;
QString deviceName = QString::fromUtf16(symbianDeviceName.Ptr(), symbianDeviceName.Length()).toUpper();
// unique address of the device
const TBTDevAddr symbianDeviceAddress = static_cast<TBTSockAddr> (m_entry().iAddr).BTAddr();
QBluetoothAddress bluetoothAddress = qTBTDevAddrToQBluetoothAddress(symbianDeviceAddress);
// format symbian major/minor numbers
quint32 deviceClass = qTPackSymbianDeviceClass(m_addr);
QBluetoothDeviceInfo deviceInfo(bluetoothAddress, deviceName, deviceClass);
if (m_addr.Rssi())
deviceInfo.setRssi(m_addr.Rssi());
#endif
if (!deviceInfo.rssi())
deviceInfo.setRssi(1);
deviceInfo.setCached(false); //TODO cache support missing from devicediscovery API
//qDebug()<< "Discovered device: name="<< deviceName <<", address=" << bluetoothAddress.toString() <<", class=" << deviceClass;
return deviceInfo;
}
