void d_swap(SEXP rx, SEXP rincx, SEXP ry, SEXP rincy)
{
int
nx, ny, n,
incx = asInteger(rincx),
incy = asInteger(rincy);
double
* x, * y;
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
n = imin2(nx, ny);
cublasDswap(n, x, incx, y, incy);
checkCublasError("d_swap");
}
void d_axpy(SEXP ralpha, SEXP rx, SEXP rincx, SEXP ry, SEXP rincy)
{
int
nx, ny, n,
incx = asInteger(rincx),
incy = asInteger(rincy);
double
alpha = asReal(ralpha),
* x, * y;
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
n = imin2(nx, ny);
cublasDaxpy(n, alpha, x, incx, y, incy);
checkCublasError("d_axpy");
}
void d_rotm(SEXP rx, SEXP rincx, SEXP ry, SEXP rincy, SEXP rsparam)
{
int
nx, ny, n, ns,
incx = asInteger(rincx),
incy = asInteger(rincy);
double
* sparam,
* x, * y;
unpackVector(rsparam, &ns, &sparam);
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
n = imin2(nx, ny);
cublasDrotm(n, x, incx, y, incy, sparam);
checkCublasError("d_rotm");
}
void d_rot(SEXP rx, SEXP rincx, SEXP ry, SEXP rincy, SEXP rsc, SEXP rss)
{
int
nx, ny, n,
incx = asInteger(rincx),
incy = asInteger(rincy);
double
sc = asReal(rsc),
ss = asReal(rss),
* x, * y;
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
n = imin2(nx, ny);
cublasDrot(n, x, incx, y, incy, sc, ss);
checkCublasError("d_rot");
}
void d_spmv(SEXP ruplo, SEXP ralpha, SEXP ra, SEXP rx, SEXP rincx,
SEXP rbeta, SEXP ry, SEXP rincy)
{
char
uplo = getSymLoc(ruplo);
double
alpha = asReal(ralpha), beta = asReal(rbeta),
* a, * x, * y;
int
rowsa, colsa,
nx, ny, incx = asInteger(rincx), incy = asInteger(rincy);
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
unpackMatrix(ra, &rowsa, &colsa, &a);
cublasDspmv(uplo, rowsa, alpha, a, x, incx, beta, y, incy);
checkCublasError("d_spmv");
}
void d_ger(SEXP ralpha, SEXP rx, SEXP rincx, SEXP ry, SEXP rincy,
SEXP ra, SEXP rlda)
{
double
alpha = asReal(ralpha),
* a, * x, * y;
int
rowsa, colsa,
lda = asInteger(rlda),
nx, ny,
incx = asInteger(rincx),
incy = asInteger(rincy);
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
unpackMatrix(ra, &rowsa, &colsa, &a);
cublasDger(rowsa, colsa, alpha, x, incx, y, incy, a, lda);
checkCublasError("d_ger");
}
SEXP d_dot(SEXP rx, SEXP rincx, SEXP ry, SEXP rincy)
{
int
nx, ny, n,
incx = asInteger(rincx),
incy = asInteger(rincy);
double
* x, * y;
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
n = imin2(nx, ny);
SEXP out;
PROTECT(out = allocVector(REALSXP, 1));
REAL(out)[0] = cublasDdot(n, x, incx, y, incy);
checkCublasError("d_dot");
UNPROTECT(1);
return out;
}
void d_gemv(SEXP rtrans, SEXP ralpha, SEXP ra, SEXP rlda, SEXP rx, SEXP rincx,
SEXP rbeta, SEXP ry, SEXP rincy)
{
char
trans = getTranspose(rtrans);
double
alpha = asReal(ralpha), beta = asReal(rbeta),
* a, * x, * y;
int
nx, ny, rowsa, colsa,
lda = asInteger(rlda),
incx = asInteger(rincx),
incy = asInteger(rincy);
unpackVector(rx, &nx, &x);
unpackVector(ry, &ny, &y);
unpackMatrix(ra, &rowsa, &colsa, &a);
cublasDgemv(trans, rowsa, colsa, alpha, a, lda, x, incx, beta, y, incy);
checkCublasError("d_gemv");
}
void d_scal(SEXP ralpha, SEXP rx, SEXP rincx)
{
int
n,
incx = asInteger(rincx);
double
* x, alpha = asReal(ralpha);
unpackVector(rx, &n, &x);
cublasDscal(n, alpha, x, incx);
checkCublasError("d_scal");
}
SEXP d_asum(SEXP vList, SEXP inc)
{
int n, increment = asInteger(inc);
double * dPtr;
unpackVector(vList, &n, &dPtr);
SEXP out;
PROTECT(out = allocVector(REALSXP, 1));
REAL(out)[0] = cublasDasum(n, dPtr, increment);
checkCublasError("d_asum");
UNPROTECT(1);
return out;
}
SEXP d_getVector(SEXP vList, SEXP inc)
{
int n, increment = asInteger(inc);
double * dPtr;
unpackVector(vList, &n, &dPtr);
SEXP out;
PROTECT(out = allocVector(REALSXP, n));
cublasGetVector(n, sizeof(double), dPtr, increment, REAL(out), 1);
checkCublasError("d_getVector");
UNPROTECT(1);
return out;
}
void d_syr(SEXP ruplo, SEXP ralpha, SEXP rx, SEXP rincx, SEXP ra, SEXP rlda)
{
char
uplo = getSymLoc(ruplo);
double
alpha = asReal(ralpha), * a, * x;
int
rowsa, colsa, lda = asInteger(rlda),
nx, incx = asInteger(rincx);
unpackVector(rx, &nx, &x);
unpackMatrix(ra, &rowsa, &colsa, &a);
cublasDsyr(uplo, rowsa, alpha, x, incx, a, lda);
checkCublasError("d_syr");
}
SEXP d_nrm2(SEXP rx, SEXP rincx)
{
int
n,
incx = asInteger(rincx);
double * x;
unpackVector(rx, &n, &x);
SEXP out;
PROTECT(out = allocVector(REALSXP, 1));
REAL(out)[0] = cublasDnrm2(n, x, incx);
checkCublasError("d_nrm2");
UNPROTECT(1);
return out;
}
void d_tpsv(SEXP ruplo, SEXP rtrans, SEXP rdiag, SEXP ra, SEXP rx, SEXP rincx)
{
char
uplo = getSymLoc(ruplo),
trans = getTranspose(rtrans),
diag = getUnitTri(rdiag);
double
* a, * x;
int
rowsa, colsa,
nx, incx = asInteger(rincx);
unpackVector(rx, &nx, &x);
unpackMatrix(ra, &rowsa, &colsa, &a);
cublasDtpsv(uplo, trans, diag, rowsa, a, x, incx);
checkCublasError("d_tpsv");
}
void vrpn_Oculus::parse_message_type_1(std::size_t bytes,
vrpn_uint8 *buffer)
{
size_t num_reports = buffer[1];
if (num_reports > 3) { num_reports = 3; }
// Skip past the report type and num_reports bytes and
// start parsing there.
vrpn_uint8 *bufptr = &buffer[2];
// The next two bytes are an increasing counter that changes by 1 for
// every report.
vrpn_uint16 report_index;
report_index = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);
channel[1] = report_index;
// The next two bytes are zero, so we skip them
vrpn_uint16 skip;
skip = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);
// The next entry is temperature, and it may be in hundredths of a degree C
vrpn_uint16 temperature;
const double temperature_scale = 0.01;
temperature = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);
channel[0] = temperature * temperature_scale;
// The magnetometer data comes after the space to store three
// reports.
vrpn_uint8 *magnetometer_ptr = &buffer[56];
vrpn_int16 magnetometer_raw[3];
for (size_t i = 0; i < 3; i++) {
magnetometer_raw[i] = vrpn_unbuffer_from_little_endian
<vrpn_int16, vrpn_uint8>(magnetometer_ptr);
}
// Invert all axes to make the magnetometer direction match
// the sign of the gravity vector.
const double magnetometer_scale = 0.0001;
channel[8] = -magnetometer_raw[0] * magnetometer_scale;
channel[9] = -magnetometer_raw[1] * magnetometer_scale;
channel[10] = -magnetometer_raw[2] * magnetometer_scale;
// Unpack a 16-byte accelerometer/gyro report using the routines from
// Oliver's code.
for (size_t i = 0; i < num_reports; i++) {
vrpn_int32 accelerometer_raw[3];
vrpn_int32 gyroscope_raw[3];
unpackVector(bufptr, accelerometer_raw);
bufptr += 8;
unpackVector(bufptr, gyroscope_raw);
bufptr += 8;
// Compute the double values using default calibration.
// The accelerometer data goes into analogs 0,1,2.
// The gyro data goes into analogs 3,4,5.
// The magnetomoter data goes into analogs 6,7,8.
const double accelerometer_scale = 0.0001;
const double gyroscope_scale = 0.0001;
channel[2] = accelerometer_raw[0] * accelerometer_scale;
channel[3] = accelerometer_raw[1] * accelerometer_scale;
channel[4] = accelerometer_raw[2] * accelerometer_scale;
channel[5] = gyroscope_raw[0] * gyroscope_scale;
channel[6] = gyroscope_raw[1] * gyroscope_scale;
channel[7] = gyroscope_raw[2] * gyroscope_scale;
vrpn_Analog::report_changes();
}
}
void vrpn_Oculus_DK2::parse_message_type_11(std::size_t bytes,
vrpn_uint8 *buffer)
{
// Started with the two different layouts in Oliver's code using my DK2
// and could not get the required data from it. I'm getting 64-byte
// reports that are somewhat like the 62-byte reports that code has but
// not the same. They seem closer to the code form the OculusRiftHIDReports.cpp
// document, but not the same (the last bytes are always 0, and they are supposed
// to be the magnetometer, for example).
// Looked at how the values changed and tried to figure out how to parse each
// part of the file.
// The first two bytes in the message are ignored; they are not present in the
// HID report for Oliver's code. The third byte is 0 and the fourth is the number
// of reports (up to 2, according to how much space is before the magnetometer
// data in the reports we found).
size_t num_reports = buffer[3];
if (num_reports > 2) { num_reports = 2; }
// @todo Check to see if we get multiple up reports
// when we get multiple other reports (I never see multiple
// other reports from my unit).
if (num_reports == 2) {
static bool printed = false;
if (!printed) {
fprintf(stderr, "vrpn_Oculus_DK2::on_data_received: Got 2 reports; check accuracy of "
"the up vector on the second and either replace with the first if it is zero or "
" remove this print statement from the code if it works.\n");
printed = true;
}
}
// Skip the first four bytes and start parsing the other reports from there.
vrpn_uint8 *bufptr = &buffer[4]; // Point at the start of the report
// The next two bytes seem to be an increasing counter that changes by 1 for
// every report.
vrpn_uint16 report_index, temperature;
report_index = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);
channel[1] = report_index;
// The next entry may be temperature, and it may be in hundredths of a degree C
const double temperature_scale = 0.01;
temperature = vrpn_unbuffer_from_little_endian<vrpn_uint16, vrpn_uint8>(bufptr);
channel[0] = temperature * temperature_scale;
// The next entry is a 4-byte counter with time since device was
// powered on in microseconds. We convert to seconds and report.
vrpn_uint32 microseconds;
microseconds = vrpn_unbuffer_from_little_endian<vrpn_uint32, vrpn_uint8>(bufptr);
channel[11] = microseconds * 1e-6;
// Unpack a 16-byte accelerometer/gyro report using the routines from
// Oliver's code. Also the magnetometer values(s).
// Then convert each to a scaled double value and send a report.
for (size_t i = 0; i < num_reports; i++) {
vrpn_int32 accelerometer_raw[3];
vrpn_int32 gyroscope_raw[3];
unpackVector(bufptr, accelerometer_raw);
bufptr += 8;
unpackVector(bufptr, gyroscope_raw);
bufptr += 8;
// Compute the double values using default calibration.
// The accelerometer data goes into analogs 0,1,2.
// The gyro data goes into analogs 3,4,5.
// The magnetomoter data goes into analogs 6,7,8.
const double accelerometer_scale = 0.0001;
const double gyroscope_scale = 0.0001;
channel[2] = accelerometer_raw[0] * accelerometer_scale;
channel[3] = accelerometer_raw[1] * accelerometer_scale;
channel[4] = accelerometer_raw[2] * accelerometer_scale;
channel[5] = gyroscope_raw[0] * gyroscope_scale;
channel[6] = gyroscope_raw[1] * gyroscope_scale;
channel[7] = gyroscope_raw[2] * gyroscope_scale;
// The magnetometer data comes after the space to store two
// reports. We don't know if there are two of them when
// there are two reports, but there is space in the report
// for it, so we try to decode two of them.
vrpn_uint8 *magnetometer_ptr = &buffer[44 + 8 * i];
vrpn_int16 magnetometer_raw[3];
for (size_t i = 0; i < 3; i++) {
magnetometer_raw[i] = vrpn_unbuffer_from_little_endian
<vrpn_int16, vrpn_uint8>(magnetometer_ptr);
}
// Invert these to make the magnetometer direction match
// the sign of the gravity vector.
const double magnetometer_scale = - 0.0001;
channel[8] = -magnetometer_raw[0] * magnetometer_scale;
channel[9] = -magnetometer_raw[1] * magnetometer_scale;
channel[10] = -magnetometer_raw[2] * magnetometer_scale;
vrpn_Analog::report_changes();
}
}
