double dist_pt_to_line_seg(const XYZ & p0, const XYZ & p1, const XYZ & p2) //p1 and p2 are the two ends of the line segment, and p0 the point
{
if (p1==p2)
return norm(p0-p1);
else if (p0==p1 || p0==p2) return 0;
XYZ d12 = p2-p1;
XYZ d01 = p1-p0;
float v01 = dot(d01, d01);
float v12 = dot(d12, d12);
float d012 = dot(d12, d01);
float t = -d012/v12;
if (t<0 || t>1) //then no intersection within the lineseg
{
double d01 = dist_L2(p0, p1);
double d02 = dist_L2(p0, p2);
return (d01<d02)?d01:d02;
}
else
{
XYZ xpt(p1.x+d12.x*t, p1.y+d12.y*t, p1.z+d12.z*t);
return dist_L2(xpt, p0);
}
}
double operator[](double x) const {
assert(!curve_.empty());
AbstractCurvePoint xpt(x);
if (xpt <= *curve_.begin()) {
return static_cast<double>(*curve_.begin());
}
if (xpt >= *curve_.rbegin()) {
return static_cast<double>(*curve_.rbegin());
}
auto it_max = curve_.upper_bound(xpt);
assert(!((*it_max) <= xpt));
assert(xpt != (*it_max));
assert(xpt < (*it_max));
auto it_min = std::prev(it_max, 1);
assert(!((*it_min) > xpt));
assert((*it_min) <= xpt);
assert((*it_min) < (*it_max));
assert((*it_min) != (*it_max));
if (xpt == (*it_min)) {
return static_cast<double>(*it_min);
}
return (*it_min).interpolate(*it_max, x);
}
double dist_pt_to_line(const XYZ & p0, const XYZ & p1, const XYZ & p2) //p1 and p2 are the two points of the straight line, and p0 the point
{
if (p1==p2)
return norm(p0-p1);
else if (p0==p1 || p0==p2) return 0;
XYZ d12 = p2-p1;
XYZ d01 = p1-p0;
float v01 = dot(d01, d01);
float v12 = dot(d12, d12);
float d012 = dot(d12, d01);
float t = -d012/v12;
XYZ xpt(p1.x+d12.x*t, p1.y+d12.y*t, p1.z+d12.z*t);
return dist_L2(xpt, p0);
}
void scsi_test_unit_ready_6(
BYTE link_to_scsi,
BYTE link_from_scsi,
BYTE target_id,
BYTE lun,
BYTE control_byte,
BYTE *message,
BYTE *msg_length,
BYTE *scsi_status,
INT16 *execution_status)
{
SCSI_CMND_6 scsi_command;
BYTE command_length;
BYTE data_transfer_mode;
BYTE rx_data[1];
INT64 rx_transfer_length;
BYTE tx_data[1];
INT64 tx_transfer_length;
rx_transfer_length = 0;
tx_transfer_length = 0;
compile_test_unit_ready_6(lun,
control_byte,
scsi_command,
&command_length,
&data_transfer_mode);
xpt(link_to_scsi,
link_from_scsi,
target_id,
command_length,
scsi_command,
data_transfer_mode,
rx_transfer_length,
rx_data,
tx_transfer_length,
tx_data,
msg_length,
message,
scsi_status,
execution_status);
}
/**
* Populate ans with the pointers from x and modify its scalar
* elements accordingly. Note that later changes to the contents of
* ans will change the contents of the SEXP.
*
* In most cases this function is called through the macros
* AS_CHM_TR() or AS_CHM_TR__(). It is unusual to call it directly.
*
* @param ans a CHM_TR pointer
* @param x pointer to an object that inherits from TsparseMatrix
* @param check_Udiag boolean - should a check for (and consequent
* expansion of) a unit diagonal be performed.
*
* @return ans containing pointers to the slots of x, *unless*
* check_Udiag and x is unitriangular.
*/
CHM_TR as_cholmod_triplet(CHM_TR ans, SEXP x, Rboolean check_Udiag)
{
static const char *valid[] = { MATRIX_VALID_Tsparse, ""};
int ctype = R_check_class_etc(x, valid),
*dims = INTEGER(GET_SLOT(x, Matrix_DimSym));
SEXP islot = GET_SLOT(x, Matrix_iSym);
int m = LENGTH(islot);
Rboolean do_Udiag = (check_Udiag && ctype % 3 == 2 && (*diag_P(x) == 'U'));
if (ctype < 0) error(_("invalid class of object to as_cholmod_triplet"));
memset(ans, 0, sizeof(cholmod_triplet)); /* zero the struct */
ans->itype = CHOLMOD_INT; /* characteristics of the system */
ans->dtype = CHOLMOD_DOUBLE;
/* nzmax, dimensions, types and slots : */
ans->nnz = ans->nzmax = m;
ans->nrow = dims[0];
ans->ncol = dims[1];
ans->stype = stype(ctype, x);
ans->xtype = xtype(ctype);
ans->i = (void *) INTEGER(islot);
ans->j = (void *) INTEGER(GET_SLOT(x, Matrix_jSym));
ans->x = xpt(ctype, x);
if(do_Udiag) {
/* diagU2N(.) "in place", similarly to Tsparse_diagU2N [./Tsparse.c]
(but without new SEXP): */
int k = m + dims[0];
CHM_TR tmp = cholmod_l_copy_triplet(ans, &cl);
int *a_i, *a_j;
if(!cholmod_reallocate_triplet((size_t) k, tmp, &cl))
error(_("as_cholmod_triplet(): could not reallocate for internal diagU2N()"
));
/* TODO? instead of copy_triplet() & reallocate_triplet()
* ---- allocate to correct length + Memcpy() here, as in
* Tsparse_diagU2N() & chTr2Ralloc() below */
a_i = tmp->i;
a_j = tmp->j;
/* add (@i, @j)[k+m] = k, @x[k+m] = 1. for k = 0,..,(n-1) */
for(k=0; k < dims[0]; k++) {
a_i[k+m] = k;
a_j[k+m] = k;
switch(ctype / 3) {
case 0: { /* "d" */
double *a_x = tmp->x;
a_x[k+m] = 1.;
break;
}
case 1: { /* "l" */
int *a_x = tmp->x;
a_x[k+m] = 1;
break;
}
case 2: /* "n" */
break;
case 3: { /* "z" */
double *a_x = tmp->x;
a_x[2*(k+m) ] = 1.;
a_x[2*(k+m)+1] = 0.;
break;
}
}
} /* for(k) */
chTr2Ralloc(ans, tmp);
cholmod_l_free_triplet(&tmp, &c);
} /* else :
* NOTE: if(*diag_P(x) == 'U'), the diagonal is lost (!);
* ---- that may be ok, e.g. if we are just converting from/to Tsparse,
* but is *not* at all ok, e.g. when used before matrix products */
return ans;
}
/**
* Populate ans with the pointers from x and modify its scalar
* elements accordingly. Note that later changes to the contents of
* ans will change the contents of the SEXP.
*
* In most cases this function is called through the macros
* AS_CHM_SP() or AS_CHM_SP__(). It is unusual to call it directly.
*
* @param ans a CHM_SP pointer
* @param x pointer to an object that inherits from CsparseMatrix
* @param check_Udiag boolean - should a check for (and consequent
* expansion of) a unit diagonal be performed.
* @param sort_in_place boolean - if the i and x slots are to be sorted
* should they be sorted in place? If the i and x slots are pointers
* to an input SEXP they should not be modified.
*
* @return ans containing pointers to the slots of x, *unless*
* check_Udiag and x is unitriangular.
*/
CHM_SP as_cholmod_sparse(CHM_SP ans, SEXP x,
Rboolean check_Udiag, Rboolean sort_in_place)
{
static const char *valid[] = { MATRIX_VALID_Csparse, ""};
int *dims = INTEGER(GET_SLOT(x, Matrix_DimSym)),
ctype = R_check_class_etc(x, valid);
SEXP islot = GET_SLOT(x, Matrix_iSym);
if (ctype < 0) error(_("invalid class of object to as_cholmod_sparse"));
if (!isValid_Csparse(x))
error(_("invalid object passed to as_cholmod_sparse"));
memset(ans, 0, sizeof(cholmod_sparse)); /* zero the struct */
ans->itype = CHOLMOD_INT; /* characteristics of the system */
ans->dtype = CHOLMOD_DOUBLE;
ans->packed = TRUE;
/* slots always present */
ans->i = INTEGER(islot);
ans->p = INTEGER(GET_SLOT(x, Matrix_pSym));
/* dimensions and nzmax */
ans->nrow = dims[0];
ans->ncol = dims[1];
/* Allow for over-allocation of the i and x slots. Needed for
* sparse X form in lme4. Right now it looks too difficult to
* check for the length of the x slot, because of the xpt
* utility, but the lengths of x and i should agree. */
ans->nzmax = LENGTH(islot);
/* values depending on ctype */
ans->x = xpt(ctype, x);
ans->stype = stype(ctype, x);
ans->xtype = xtype(ctype);
/* are the columns sorted (increasing row numbers) ?*/
ans->sorted = check_sorted_chm(ans);
if (!(ans->sorted)) { /* sort columns */
if(sort_in_place) {
if (!cholmod_sort(ans, &c))
error(_("in_place cholmod_sort returned an error code"));
ans->sorted = 1;
}
else {
CHM_SP tmp = cholmod_copy_sparse(ans, &c);
if (!cholmod_sort(tmp, &c))
error(_("cholmod_sort returned an error code"));
#ifdef DEBUG_Matrix
/* This "triggers" exactly for return values of dtCMatrix_sparse_solve():*/
/* Don't want to translate this: want it report */
Rprintf("Note: as_cholmod_sparse() needed cholmod_sort()ing\n");
#endif
chm2Ralloc(ans, tmp);
cholmod_free_sparse(&tmp, &c);
}
}
if (check_Udiag && ctype % 3 == 2 // triangular
&& (*diag_P(x) == 'U')) { /* diagU2N(.) "in place" : */
double one[] = {1, 0};
CHM_SP eye = cholmod_speye(ans->nrow, ans->ncol, ans->xtype, &c);
CHM_SP tmp = cholmod_add(ans, eye, one, one, TRUE, TRUE, &c);
#ifdef DEBUG_Matrix_verbose /* happens quite often, e.g. in ../tests/indexing.R : */
Rprintf("Note: as_cholmod_sparse(<ctype=%d>) - diagU2N\n", ctype);
#endif
chm2Ralloc(ans, tmp);
cholmod_free_sparse(&tmp, &c);
cholmod_free_sparse(&eye, &c);
} /* else :
* NOTE: if(*diag_P(x) == 'U'), the diagonal is lost (!);
* ---- that may be ok, e.g. if we are just converting from/to Tsparse,
* but is *not* at all ok, e.g. when used before matrix products */
return ans;
}
