Eigen::Matrix<T,4,1> rotate_from_xy(Eigen::Matrix<T,4,1> norm,
Eigen::Matrix<T,4,1> cen,
Eigen::Matrix<T,4,1> vec){
Eigen::Matrix<T,4,1> vz(0,0,1);
norm.normalize();
Eigen::Matrix<T,4,1> xe = -cross(norm,vz).normalize();
T r1 = norm.mag();
T o = acos(dot(vz,norm)/r1);
Eigen::Quaternion<T> dq(cos(o/2.), sin(o/2.)*xe[0], sin(o/2.)*xe[1], sin(o/2.)*xe[2]);
Eigen::Matrix<T,4,1> c = dq.rotate(vec);
c += cen;
return c;
}
void TransformNDOF::GetJacobian(const scalarArray &q, se3Array &J)
{
SE3 T = GetTransform(q);
scalarArray dq(m_iDOF);
for ( int i = 0; i < m_iDOF; i++ ) dq[i] = q[i];
for ( int i = 0; i < m_iDOF; i++ )
{
dq[i] += m_rEPS;
J[i] = (SCALAR_1 / m_rEPS) * Linearize(T % GetTransform(dq));
dq[i] -= m_rEPS;
}
}
/*
Most pages have similar items, we add their generic data to the JSON
string here.
*/
QString CreateNewDatabase::genericAddData() {
std::unique_ptr<QString> ss(new QString(""));
QString dq("\"");
QTextStream s(ss.get());
bool un = false;
bool nu = false;
QString rowname;
switch(ui->stackedWidget->currentIndex())
{
case CreateNewDatabase::text:
rowname = ui->txt_grp_rowname->text();
un = ui->txt_grp_unique->isChecked();
nu = ui->txt_grp_isnull->isChecked();
break;
case CreateNewDatabase::choice:
rowname = ui->choice_grp_rowname->text();
un = ui->choice_grp_unique->isChecked();
nu = ui->choice_grp_isnull->isChecked();
break;
case CreateNewDatabase::date:
rowname = ui->date_grp_rowname->text();
un = ui->date_grp_unique->isChecked();
nu = ui->date_grp_isnull->isChecked();
break;
case CreateNewDatabase::time:
std::runtime_error("Adding time rows is not implemented");
break;
case CreateNewDatabase::group:
std::runtime_error("Adding group rows is not implemented");
break;
case CreateNewDatabase::currency:
rowname = ui->curr_grp_rowname->text();
un = ui->curr_grp_unique->isChecked();
nu = ui->curr_grp_isnull->isChecked();
break;
default:
std::runtime_error("Index out of bounds on the buttonGroup, you also need to add the element to the class enum");
break;
}
s << quote(rowname) << ":{"
// we're using QStrings, so %1, %2 etc refer to positional
// arguments. They can be filled in with QString::arg(...)
<< quote("ROWNUM") << ":" << "%1" << ","
<< quote("ROWDATA") << ":{"
<< quote("UNIQUE") << ":" << toStrBool(un) << ","
<< quote("NULL") << ":" << toStrBool(nu) << ",";
return *s.string();
}
void TransformNDOF::GetHessian(const scalarArray &q, se3DbAry &H)
{
se3Array J(m_iDOF), dJ(m_iDOF);
GetJacobian(q, J);
scalarArray dq(m_iDOF);
for ( int i = 0; i < m_iDOF; i++ ) dq[i] = q[i];
for ( int i = 0; i < m_iDOF; i++ )
{
dq[i] += m_rEPS;
GetJacobian(dq, dJ);
for ( int j = i + 1; j < m_iDOF; j++ ) H[i][j] = (SCALAR_1 / m_rEPS) * (dJ[j] - J[j]);
dq[i] -= m_rEPS;
}
}
bool RTAStar::collisionFree(const Eigen::VectorXd &q_from, const Eigen::VectorXd &dq_from, const KDL::Frame &x_from, const KDL::Frame &x_to, int try_idx, Eigen::VectorXd &q_to, Eigen::VectorXd &dq_to, KDL::Frame &x_to_out,
std::list<KDL::Frame > *path_x, std::list<Eigen::VectorXd > *path_q) const {
path_x->clear();
path_q->clear();
Eigen::VectorXd q(ndof_), dq(ndof_), ddq(ndof_);
ddq.setZero();
sim_->setTarget(x_to);
sim_->setState(q_from, dq_from, ddq);
KDL::Twist diff_target = KDL::diff(x_from, x_to, 1.0);
for (int loop_counter = 0; loop_counter < 1500; loop_counter++) {
// Eigen::VectorXd q(ndof_), dq(ndof_), ddq(ndof_);
sim_->getState(q, dq, ddq);
KDL::Frame r_HAND_current;
kin_model_->calculateFk(r_HAND_current, effector_name_, q);
KDL::Twist prev_diff;
if (path_x->empty()) {
prev_diff = KDL::diff(x_from, r_HAND_current, 1.0);
}
else {
prev_diff = KDL::diff(path_x->back(), r_HAND_current, 1.0);
}
if (prev_diff.vel.Norm() > 0.1 || prev_diff.rot.Norm() > 30.0/180.0*3.1415) {
path_x->push_back(r_HAND_current);
path_q->push_back(q);
}
KDL::Twist goal_diff( KDL::diff(r_HAND_current, x_to, 1.0) );
if (goal_diff.vel.Norm() < 0.01) {// && goal_diff.rot.Norm() < 5.0/180.0*3.1415) {
q_to = q;
dq_to = dq;
x_to_out = r_HAND_current;
return true;
}
sim_->oneStep();
if (sim_->inCollision()) {
return false;
}
}
return false;
}
static dFloat PlaneMeshCollisionRayHitCallback (NewtonUserMeshCollisionRayHitDesc* const rayDesc)
{
dVector q0 (rayDesc->m_p0[0], rayDesc->m_p0[1], rayDesc->m_p0[2]);
dVector q1 (rayDesc->m_p1[0], rayDesc->m_p1[1], rayDesc->m_p1[2]);
dVector dq (q1 - q0);
// calculate intersection between point lien a plane and return intersection parameter
dInfinitePlane* const me = (dInfinitePlane*) rayDesc->m_userData;
dFloat t = -(me->m_plane.DotProduct3(q0) + me->m_plane.m_w) / (me->m_plane.DotProduct3(dq));
if ((t > 0.0f) && (t < 1.0f)) {
rayDesc->m_normalOut[0] = me->m_plane[0];
rayDesc->m_normalOut[1] = me->m_plane[1];
rayDesc->m_normalOut[2] = me->m_plane[2];
} else {
t = 1.2f;
}
return t;
}
Eigen::Matrix<T,4,1> rotate_to_plane(Eigen::Matrix<T,4,1> normP,
Eigen::Matrix<T,4,1> norm,
Eigen::Matrix<T,4,1> cen,
Eigen::Matrix<T,4,1> vec){
norm.normalize();
normP.normalize();
Eigen::Matrix<T,4,1> xe = cross(norm,normP).normalize();
T r1 = norm.mag();
T rP = normP.mag();
T o = acos(dot(normP,norm)/r1/rP) - M_PI;
T o0 = acos(dot(normP,norm)/r1/rP);
Eigen::Quaternion<T> qP(normP);
Eigen::Quaternion<T> qi(norm);
// Eigen::Quaternion<T> dq = (qP-qi).normalize();
Eigen::Quaternion<T> dq(cos(o/2.), sin(o/2.)*xe[0], sin(o/2.)*xe[1], sin(o/2.)*xe[2]);
// Eigen::Quaternion<T> dq = qP*qi.conj()*qP;
Eigen::Matrix<T,4,1> c = dq.normalize().rotate(vec - cen);
return c;
}
dgVector dgQuaternion::CalcAverageOmega (const dgQuaternion &q1, dgFloat32 invdt) const
{
dgQuaternion q0 (*this);
if (q0.DotProduct (q1) < 0.0f) {
q0.Scale(-1.0f);
}
dgQuaternion dq (q0.Inverse() * q1);
dgVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3, dgFloat32 (0.0f));
dgFloat32 dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dgFloat32(dgFloat32 (1.0e-5f) * dgFloat32 (1.0e-5f))) {
return dgVector (dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
}
dgFloat32 dirMagInv = dgRsqrt (dirMag2);
dgFloat32 dirMag = dirMag2 * dirMagInv;
dgFloat32 omegaMag = dgFloat32(2.0f) * dgAtan2 (dirMag, dq.m_q0) * invdt;
return omegaDir.Scale3 (dirMagInv * omegaMag);
}
dVector dQuaternion::CalcAverageOmega (const dQuaternion &q1, dFloat invdt) const
{
dQuaternion q0 (*this);
if (q0.DotProduct (q1) < 0.0f) {
q0.Scale(-1.0f);
}
dQuaternion dq (q0.Inverse() * q1);
dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);
dFloat dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
return dVector (dFloat(0.0f), dFloat(0.0f), dFloat(0.0f), dFloat(0.0f));
}
dFloat dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
dFloat dirMag = dirMag2 * dirMagInv;
dFloat omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) * invdt;
return omegaDir.Scale (dirMagInv * omegaMag);
}
void printlevel(struct BT *root)
{
if(!root)
return;
struct Q *q=createQ(MAX);
nq(q,root);
while(!isEmpty(q))
{
root=dq(q);
printf("[%d]\t",root->data);
if(root->l)
nq(q,root->l);
if(root->r)
nq(q,root->r);
}
}
/*
Generates the json string for the CHOICE type.
*/
QString CreateNewDatabase::generateChoiceData() {
std::unique_ptr<QString> ss(new QString(""));
QString dq("\"");
QTextStream s(ss.get());
QStringList choices = ui->choice_grp_choices->toPlainText().split("\n");
choices.removeDuplicates();
s << genericAddData()
<< quote("TYPE", "CHOICE") << ","
<< quote("CHOICES") << ":[";
for (int x = 0; x < choices.size(); ++x) {
s << dq << choices[x] << dq;
if (x + 1 != choices.size())
s << ",";
}
s << "]}}";
return *s.string();
}
void DynamicChain2D::GetCoriolisForceMatrix(Matrix& C)
{
std::vector<Matrix> dBdi(q.n);
for(int z=0;z<q.n;z++)
GetKineticEnergyMatrixDeriv(z,dBdi[z]);
C.resize(q.n,q.n);
Update_dB_dq();
for(int i=0;i<q.n;i++) {
for(int j=0;j<q.n;j++) {
C(i,j) = Zero;
for(int k=0;k<q.n;k++) {
Real dbij_dqk,dbik_dqj,dbjk_dqi;
dbij_dqk=dB_dq[k](i,j);
dbik_dqj=dB_dq[j](i,k);
dbjk_dqi=dB_dq[i](j,k);
C(i,j) += (dbij_dqk + dbik_dqj - dbjk_dqi)*dq(k);
}
C(i,j)*=Half;
}
}
}
int test_inverse()
{
int Error(0);
float const Epsilon = 0.0001f;
glm::dualquat dqid;
glm::mat4x4 mid(1.0f);
for (int j = 0; j < 100; ++j)
{
glm::mat4x4 rot = glm::yawPitchRoll(myfrand() * 360.0f, myfrand() * 360.0f, myfrand() * 360.0f);
glm::vec3 vt = glm::vec3(myfrand() * 10.0f, myfrand() * 10.0f, myfrand() * 10.0f);
glm::mat4x4 m = glm::translate(mid, vt) * rot;
glm::quat qr = glm::quat_cast(m);
glm::dualquat dq(qr);
glm::dualquat invdq = glm::inverse(dq);
glm::dualquat r1 = invdq * dq;
glm::dualquat r2 = dq * invdq;
Error += glm::all(glm::epsilonEqual(r1.real, dqid.real, Epsilon)) && glm::all(glm::epsilonEqual(r1.dual, dqid.dual, Epsilon)) ? 0 : 1;
Error += glm::all(glm::epsilonEqual(r2.real, dqid.real, Epsilon)) && glm::all(glm::epsilonEqual(r2.dual, dqid.dual, Epsilon)) ? 0 : 1;
// testing commutative property
glm::dualquat r ( glm::quat( myfrand() * glm::pi<float>() * 2.0f, myfrand(), myfrand(), myfrand() ),
glm::vec3(myfrand() * 10.0f, myfrand() * 10.0f, myfrand() * 10.0f) );
glm::dualquat riq = (r * invdq) * dq;
glm::dualquat rqi = (r * dq) * invdq;
Error += glm::all(glm::epsilonEqual(riq.real, rqi.real, Epsilon)) && glm::all(glm::epsilonEqual(riq.dual, rqi.dual, Epsilon)) ? 0 : 1;
}
return Error;
}
void FreeJoint::updateJacobianTimeDeriv() {
Eigen::Vector3d q(mCoordinate[0].get_q(),
mCoordinate[1].get_q(),
mCoordinate[2].get_q());
Eigen::Vector3d dq(mCoordinate[0].get_dq(),
mCoordinate[1].get_dq(),
mCoordinate[2].get_dq());
Eigen::Matrix3d dJ = math::expMapJacDot(q, dq);
Eigen::Vector6d dJ0;
Eigen::Vector6d dJ1;
Eigen::Vector6d dJ2;
Eigen::Vector6d J3;
Eigen::Vector6d J4;
Eigen::Vector6d J5;
dJ0 << dJ(0, 0), dJ(0, 1), dJ(0, 2), 0, 0, 0;
dJ1 << dJ(1, 0), dJ(1, 1), dJ(1, 2), 0, 0, 0;
dJ2 << dJ(2, 0), dJ(2, 1), dJ(2, 2), 0, 0, 0;
J3 << 0, 0, 0, 1, 0, 0;
J4 << 0, 0, 0, 0, 1, 0;
J5 << 0, 0, 0, 0, 0, 1;
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
mdS.col(3) =
-math::ad(mS.leftCols<3>() * get_dq().head<3>(),
math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3));
mdS.col(4) =
-math::ad(mS.leftCols<3>() * get_dq().head<3>(),
math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4));
mdS.col(5) =
-math::ad(mS.leftCols<3>() * get_dq().head<3>(),
math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5));
assert(!math::isNan(mdS));
}
void bfsIterative(int node){
visited[node] = 1;
inq(node);
// visited[node] = 1;
// printf("%c ", dic[node]);
while(start < end){
//printf("%d %d\n", start , end);
int next_node = dq();
printf("%c ", dic[next_node]);
int i;
for(i = A; i <= G; i++) {
if(!visited[i] && g[next_node][i]) {
visited[i] = 1;
inq(i);
}
}
// int j;
// for(j = 0; j <= MAX_N; j++) printf("%c ", dic[Q[j]]);
// printf("\n");
}
}
dVector dQuaternion::CalcAverageOmega (const dQuaternion &QB, dFloat dt) const
{
dFloat dirMag;
dFloat dirMag2;
dFloat omegaMag;
dFloat dirMagInv;
dQuaternion dq (Inverse() * QB);
dVector omegaDir (dq.m_q1, dq.m_q2, dq.m_q3);
dirMag2 = omegaDir % omegaDir;
if (dirMag2 < dFloat(dFloat (1.0e-5f) * dFloat (1.0e-5f))) {
return dVector (dFloat(0.0f), dFloat(0.0f), dFloat(0.0f), dFloat(0.0f));
}
dirMagInv = dFloat (1.0f) / dSqrt (dirMag2);
dirMag = dirMag2 * dirMagInv;
omegaMag = dFloat(2.0f) * dAtan2 (dirMag, dq.m_q0) / dt;
return omegaDir.Scale (dirMagInv * omegaMag);
}
inline void BallJoint::updateJacobianTimeDeriv()
{
Eigen::Vector3d q(mCoordinate[0].get_q(),
mCoordinate[1].get_q(),
mCoordinate[2].get_q());
Eigen::Vector3d dq(mCoordinate[0].get_dq(),
mCoordinate[1].get_dq(),
mCoordinate[2].get_dq());
Eigen::Matrix3d dJ = math::expMapJacDot(q, dq);
Eigen::Vector6d dJ0;
Eigen::Vector6d dJ1;
Eigen::Vector6d dJ2;
dJ0 << dJ(0,0), dJ(0,1), dJ(0,2), 0, 0, 0;
dJ1 << dJ(1,0), dJ(1,1), dJ(1,2), 0, 0, 0;
dJ2 << dJ(2,0), dJ(2,1), dJ(2,2), 0, 0, 0;
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
}
void insert(struct BT **root,struct Q *q,int d)
{
struct BT *temp=nn(d);
if(!*root)
*root=temp;
else
{
struct BT *front=get(q);
if(!front->l)
front->l=temp;
else if(!front->r)
front->r=temp;
if(hasdouble(front))
dq(q);
}
nq(q,temp);
}
void FreeJoint::_updateAcceleration()
{
// dS
Eigen::Vector3d q(mCoordinate[0].get_q(),
mCoordinate[1].get_q(),
mCoordinate[2].get_q());
Eigen::Vector3d dq(mCoordinate[0].get_dq(),
mCoordinate[1].get_dq(),
mCoordinate[2].get_dq());
Eigen::Matrix3d dJ = math::expMapJacDot(q, dq);
Eigen::Vector6d dJ0;
Eigen::Vector6d dJ1;
Eigen::Vector6d dJ2;
Eigen::Vector6d J3;
Eigen::Vector6d J4;
Eigen::Vector6d J5;
dJ0 << dJ(0,0), dJ(0,1), dJ(0,2), 0, 0, 0;
dJ1 << dJ(1,0), dJ(1,1), dJ(1,2), 0, 0, 0;
dJ2 << dJ(2,0), dJ(2,1), dJ(2,2), 0, 0, 0;
J3 << 0, 0, 0, 1, 0, 0;
J4 << 0, 0, 0, 0, 1, 0;
J5 << 0, 0, 0, 0, 0, 1;
mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
mdS.col(3) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3));
mdS.col(4) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4));
mdS.col(5) = -math::ad(mS.leftCols<3>() * get_dq().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5));
// dV = dS * dq + S * ddq
mdV = mdS * get_dq() + mS * get_ddq();
}
RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id)
{
m_loop++;
// make timestamp
coil::TimeValue coiltm(coil::gettimeofday());
hrp::dvector dq(m_robot->numJoints());
RTC::Time tm;
tm.sec = coiltm.sec();
tm.nsec = coiltm.usec()*1000;
// update port
if (m_tauCurrentInIn.isNew()) {
m_tauCurrentInIn.read();
}
if (m_tauMaxInIn.isNew()) {
m_tauMaxInIn.read();
}
if (m_qCurrentInIn.isNew()) {
m_qCurrentInIn.read();
}
if (m_qRefInIn.isNew()) {
m_qRefInIn.read();
}
if (m_qRefIn.data.length() == m_robot->numJoints() &&
m_tauCurrentIn.data.length() == m_robot->numJoints() &&
m_qCurrentIn.data.length() == m_robot->numJoints()) {
// update model
for ( int i = 0; i < m_robot->numJoints(); i++ ){
m_robot->joint(i)->q = m_qCurrentIn.data[i];
}
m_robot->calcForwardKinematics();
// calculate dq by torque controller
executeTorqueControl(dq);
// output restricted qRef
for (int i = 0; i < m_robot->numJoints(); i++) {
m_qRefOut.data[i] = std::min(std::max(m_qRefIn.data[i] + dq[i], m_robot->joint(i)->llimit), m_robot->joint(i)->ulimit);
}
} else {
if (isDebug()) {
std::cerr << "TorqueController input is not correct" << std::endl;
std::cerr << " numJoints: " << m_robot->numJoints() << std::endl;
std::cerr << " qCurrent: " << m_qCurrentIn.data.length() << std::endl;
std::cerr << " qRefIn: " << m_qRefIn.data.length() << std::endl;
std::cerr << "tauCurrent: " << m_tauCurrentIn.data.length() << std::endl;
std::cerr << std::endl;
}
// pass qRefIn to qRefOut
for (int i = 0; i < m_robot->numJoints(); i++) {
m_qRefOut.data[i] = m_qRefIn.data[i];
}
}
m_qRefOut.tm = tm;
m_qRefOutOut.write();
return RTC::RTC_OK;
}
bool add(control_ptr ob, long i, long j, T teni, T tenj){
// m2::subdivide<T> sub;
// m2Ch = sub.subdivide_control(m2Ch);
//bezier_curve<point_space<3,T> > mCurve(3);
face_ptr fi = ob->face(i);
face_ptr fj = ob->face(j);
bezier_curve<point_space<3,typename SPACE::type> > mCurve(4);
coordinate_type c0 = fi->calculate_center();
coordinate_type c0t = c0 + teni*ob->face(i)->normal();
coordinate_type c1 = fj->calculate_center();
coordinate_type c1t = c1 + tenj*ob->face(j)->normal();
if (fi->size() != fj->size() ) {
return false;
}
mCurve.push_point(c0);
mCurve.push_point(c0t);
mCurve.push_point(c1t);
mCurve.push_point(c1);
mCurve.update_knot_vector();
Eigen::Quaternion<T> q1(ob->face(j)->normal());q1.normalize();
vector<coordinate_type > f0 = fi->coordinate_trace();
vector<coordinate_type > f1 = fj->coordinate_trace();
long Ni = mCurve.mKnots.size();
T dt = T(1./T(Ni-1));
T t = dt*0.5;
// for(long ii = 0; ii < 3; ++ii){
for(long ii = 0; ii < Ni-1; ++ii){
coordinate_type Ni = fi->normal();
Eigen::Quaternion<T> q0(Ni); q0.normalize();
coordinate_type de = mCurve.mKnots[ii]-mCurve.mKnots[ii+1];
de.normalize();
Eigen::Quaternion<T> qi(de); qi.normalize();
//
coordinate_type xe = cross(Ni,-de).normalize();
T r0 = de.mag();
T r1 = Ni.mag();
T o = acos(dot(-de,Ni)/r0/r1);
Eigen::Quaternion<T> dq(cos(o/2.), sin(o/2.)*xe[0], sin(o/2.)*xe[1], sin(o/2.)*xe[2]);
// Eigen::Quaternion<T> dq = (qi*q0); //dq[0]*=M_PI;
//dq.normalize();
coordinate_type cen = fi->calculate_center();
face_vertex_ptr itb = fi->fbegin();
face_vertex_ptr ite = fi->fend();
construct<SPACE> bevel;
bevel.bevel_face(ob, fi, 0.0, 0.0);
bool at_head = false;
while (!at_head) {
at_head = itb == ite;
coordinate_type c = itb->coordinate();
coordinate_type cp0 = dq.rotate(c-cen);
itb->coordinate() = cp0 + (mCurve.mKnots[ii+1]);
itb = itb->next();
}
fi->update_normal();
fi->update_center();
this->slerp_face(t, fi, fj);
// cen = fi->calculate_center();
// itb = fi->fbegin();
// at_head = false;
// while (!at_head) {
// at_head = itb == ite;
// coordinate_type c = itb->coordinate();
// coordinate_type N = fi->normal();
// T o = 0.5*t*M_PI;
// Eigen::Quaternion<T> qb(cos(o*0.5), sin(o*0.5)*N[0], sin(o*0.5)*N[1], sin(o*0.5)*N[2]);
// qb.normalize();
// Eigen::Matrix<T,4,1> cp1 = qb.rotate(c-cen);
// // Eigen::Matrix<T,4,1> cp1 = cp0;
// // itb->coordinate() = cp0 + (mCurve.mKnots[ii] + mCurve.mKnots[ii + 1])*0.5;
// itb->coordinate() = cp1 + (mCurve.mKnots[ii+1]);
// itb = itb->next();
// }
t += dt;
}
graph_skinning<SPACE> gs; //TODO: stitch_faces needs to get moved to a surgery object
gs.stitch_faces(ob, fi, fj, 0.01);
return true;
}
dgFloat32 dgCollisionChamferCylinder::RayCast (const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
if (q0.m_x > m_height) {
if (q1.m_x < m_height) {
dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector (dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
//contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
if (q0.m_x < -m_height) {
if (q1.m_x > -m_height) {
dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector (dgFloat32 (-1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
//contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
dgVector dq ((q1 - q0) & dgVector::m_triplexMask);
// avoid NaN as a result of a division by zero
if ((dq % dq) <= 0.0f) {
return dgFloat32(1.2f);
}
//dgVector dir (dq.Scale3 (dgRsqrt(dq % dq)));
dgVector dir (dq.CompProduct4 (dq.InvMagSqrt()));
if (dgAbsf (dir.m_x) > 0.9999f) {
return dgCollisionConvex::RayCast (q0, q1, maxT, contactOut, NULL, NULL, NULL);
}
dgVector p0 (q0);
dgVector p1 (q1);
dgVector dp (dq);
p0.m_x = dgFloat32 (0.0f);
p1.m_x = dgFloat32 (0.0f);
dp.m_x = dgFloat32 (0.0f);
dgFloat32 a = dp % dp;
//dgFloat32 b = dgFloat32 (2.0f) * (p0 % dp);
dgFloat32 b = dgFloat32 (2.0f) * (p0.DotProduct4(dp).GetScalar());
dgFloat32 r = m_radius + m_height;
dgFloat32 c = p0 % p0 - r * r;
dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
if (desc >= 0.0f) {
desc = dgSqrt (desc);
dgFloat32 den = dgFloat32 (0.5f) / a;
dgFloat32 s0 = (-b + desc) * den;
dgFloat32 s1 = (-b - desc) * den;
dgVector origin0 (p0 + dp.Scale4 (s0));
dgVector origin1 (p0 + dp.Scale4 (s1));
dgFloat32 s = m_radius / (m_radius + m_height);
origin0 = origin0.Scale4 (s);
origin1 = origin1.Scale4 (s);
dgFloat32 t0 = dgRayCastSphere (q0, q1, origin0, m_height);
dgFloat32 t1 = dgRayCastSphere (q0, q1, origin1, m_height);
if (t0 < t1) {
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale4 (t0) - origin0;
dgAssert (contactOut.m_normal.m_w == dgFloat32 (0.0f));
//contactOut.m_normal = contactOut.m_normal.Scale3 (dgRsqrt (contactOut.m_normal % contactOut.m_normal));
contactOut.m_normal = contactOut.m_normal.CompProduct4(contactOut.m_normal.DotProduct4(contactOut.m_normal).InvSqrt());
//contactOut.m_userId = SetUserDataID();
return t0;
}
} else {
if ((t1 >= 0.0f) && (t1 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale4 (t1) - origin1;
dgAssert (contactOut.m_normal.m_w == dgFloat32 (0.0f));
//contactOut.m_normal = contactOut.m_normal.Scale3 (dgRsqrt (contactOut.m_normal % contactOut.m_normal));
contactOut.m_normal = contactOut.m_normal.CompProduct4(contactOut.m_normal.DotProduct4(contactOut.m_normal).InvSqrt());
//contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
return dgFloat32 (1.2f);
}
void AnimeshObject::Skin ()
{
if (!skeleton)
return;
CS_ASSERT (SkinV ?
skinnedVertices->GetElementCount () >= factory->vertexCount : true);
CS_ASSERT (SkinN ?
skinnedNormals->GetElementCount () >= factory->vertexCount : true);
CS_ASSERT (SkinTB ?
skinnedTangents->GetElementCount () >= factory->vertexCount
&& skinnedBinormals->GetElementCount () >= factory->vertexCount
: true);
// Setup some local data
csVertexListWalker<float, csVector3> srcVerts (postMorphVertices);
csRenderBufferLock<csVector3> dstVerts (skinnedVertices);
csVertexListWalker<float, csVector3> srcNormals (factory->normalBuffer);
csRenderBufferLock<csVector3> dstNormals (skinnedNormals);
csVertexListWalker<float, csVector3> srcTangents (factory->tangentBuffer);
csRenderBufferLock<csVector3> dstTangents (skinnedTangents);
csVertexListWalker<float, csVector3> srcBinormals (factory->binormalBuffer);
csRenderBufferLock<csVector3> dstBinormals (skinnedBinormals);
csSkeletalState2* skeletonState = lastSkeletonState;
csAnimatedMeshBoneInfluence* influence = factory->boneInfluences.GetArray ();
for (size_t i = 0; i < factory->vertexCount; ++i)
{
// Accumulate data for the vertex
int numInfluences = 0;
csDualQuaternion dq (csQuaternion (0,0,0,0), csQuaternion (0,0,0,0));
csQuaternion pivot;
for (size_t j = 0; j < 4; ++j, ++influence) // @@SOLVE 4
{
if (influence->influenceWeight > 0.0f)
{
numInfluences++;
csDualQuaternion inflQuat (
skeletonState->GetQuaternion (influence->bone),
skeletonState->GetVector (influence->bone));
if (numInfluences == 1)
{
pivot = inflQuat.real;
}
else if (inflQuat.real.Dot (pivot) < 0.0f)
{
inflQuat *= -1.0f;
}
dq += inflQuat * influence->influenceWeight;
}
}
if (numInfluences == 0)
{
if (SkinV)
{
dstVerts[i] = *srcVerts;
}
if (SkinN)
{
dstNormals[i] = *srcNormals;
}
if (SkinTB)
{
dstTangents[i] = *srcTangents;
dstBinormals[i] = *srcBinormals;
}
}
else
{
dq = dq.Unit ();
if (SkinV)
{
dstVerts[i] = dq.TransformPoint (*srcVerts);
}
if (SkinN)
{
dstNormals[i] = dq.Transform (*srcNormals);
}
if (SkinTB)
{
dstTangents[i] = dq.Transform (*srcTangents);
dstBinormals[i] = dq.Transform (*srcBinormals);
}
}
++srcVerts;
++srcNormals;
++srcTangents;
++srcBinormals;
}
}
void
UZRectMollerup::tick (const GeometryRect& geo,
const std::vector<size_t>& drain_cell,
const double drain_water_level,
const Soil& soil,
SoilWater& soil_water, const SoilHeat& soil_heat,
const Surface& surface, const Groundwater& groundwater,
const double dt, Treelog& msg)
{
daisy_assert (K_average.get ());
const size_t edge_size = geo.edge_size (); // number of edges
const size_t cell_size = geo.cell_size (); // number of cells
// Insert magic here.
ublas::vector<double> Theta (cell_size); // water content
ublas::vector<double> Theta_previous (cell_size); // at start of small t-step
ublas::vector<double> h (cell_size); // matrix pressure
ublas::vector<double> h_previous (cell_size); // at start of small timestep
ublas::vector<double> h_ice (cell_size); //
ublas::vector<double> S (cell_size); // sink term
ublas::vector<double> S_vol (cell_size); // sink term
#ifdef TEST_OM_DEN_ER_BRUGT
ublas::vector<double> S_macro (cell_size); // sink term
std::vector<double> S_drain (cell_size, 0.0); // matrix-> macro -> drain flow
std::vector<double> S_drain_sum (cell_size, 0.0); // For large timestep
const std::vector<double> S_matrix (cell_size, 0.0); // matrix -> macro
std::vector<double> S_matrix_sum (cell_size, 0.0); // for large timestep
#endif
ublas::vector<double> T (cell_size); // temperature
ublas::vector<double> Kold (edge_size); // old hydraulic conductivity
ublas::vector<double> Ksum (edge_size); // Hansen hydraulic conductivity
ublas::vector<double> Kcell (cell_size); // hydraulic conductivity
ublas::vector<double> Kold_cell (cell_size); // old hydraulic conductivity
ublas::vector<double> Ksum_cell (cell_size); // Hansen hydraulic conductivity
ublas::vector<double> h_lysimeter (cell_size);
std::vector<bool> active_lysimeter (cell_size);
const std::vector<size_t>& edge_above = geo.cell_edges (Geometry::cell_above);
const size_t edge_above_size = edge_above.size ();
ublas::vector<double> remaining_water (edge_above_size);
std::vector<bool> drain_cell_on (drain_cell.size (),false);
for (size_t i = 0; i < edge_above_size; i++)
{
const size_t edge = edge_above[i];
remaining_water (i) = surface.h_top (geo, edge);
}
ublas::vector<double> q; // Accumulated flux
q = ublas::zero_vector<double> (edge_size);
ublas::vector<double> dq (edge_size); // Flux in small timestep.
dq = ublas::zero_vector<double> (edge_size);
//Make Qmat area diagonal matrix
//Note: This only needs to be calculated once...
ublas::banded_matrix<double> Qmat (cell_size, cell_size, 0, 0);
for (int c = 0; c < cell_size; c++)
Qmat (c, c) = geo.cell_volume (c);
// make vectors
for (size_t cell = 0; cell != cell_size ; ++cell)
{
Theta (cell) = soil_water.Theta (cell);
h (cell) = soil_water.h (cell);
h_ice (cell) = soil_water.h_ice (cell);
S (cell) = soil_water.S_sum (cell);
S_vol (cell) = S (cell) * geo.cell_volume (cell);
if (use_forced_T)
T (cell) = forced_T;
else
T (cell) = soil_heat.T (cell);
h_lysimeter (cell) = geo.zplus (cell) - geo.cell_z (cell);
}
// Remember old value.
Theta_error = Theta;
// Start time loop
double time_left = dt; // How much of the large time step left.
double ddt = dt; // We start with small == large time step.
int number_of_time_step_reductions = 0;
int iterations_with_this_time_step = 0;
int n_small_time_steps = 0;
while (time_left > 0.0)
{
if (ddt > time_left)
ddt = time_left;
std::ostringstream tmp_ddt;
tmp_ddt << "Time t = " << (dt - time_left)
<< "; ddt = " << ddt
<< "; steps " << n_small_time_steps
<< "; time left = " << time_left;
Treelog::Open nest (msg, tmp_ddt.str ());
if (n_small_time_steps > 0
&& (n_small_time_steps%msg_number_of_small_time_steps) == 0)
{
msg.touch ();
msg.flush ();
}
n_small_time_steps++;
if (n_small_time_steps > max_number_of_small_time_steps)
{
msg.debug ("Too many small timesteps");
throw "Too many small timesteps";
}
// Initialization for each small time step.
if (debug > 0)
{
std::ostringstream tmp;
tmp << "h = " << h << "\n";
tmp << "Theta = " << Theta;
msg.message (tmp.str ());
}
int iterations_used = 0;
h_previous = h;
Theta_previous = Theta;
if (debug == 5)
{
std::ostringstream tmp;
tmp << "Remaining water at start: " << remaining_water;
msg.message (tmp.str ());
}
ublas::vector<double> h_conv;
for (size_t cell = 0; cell != cell_size ; ++cell)
active_lysimeter[cell] = h (cell) > h_lysimeter (cell);
for (size_t edge = 0; edge != edge_size ; ++edge)
{
Kold[edge] = find_K_edge (soil, geo, edge, h, h_ice, h_previous, T);
Ksum [edge] = 0.0;
}
std::vector<top_state> state (edge_above.size (), top_undecided);
// We try harder with smaller timesteps.
const int max_loop_iter
= max_iterations * (number_of_time_step_reductions
* max_iterations_timestep_reduction_factor + 1);
do // Start iteration loop
{
h_conv = h;
iterations_used++;
std::ostringstream tmp_conv;
tmp_conv << "Convergence " << iterations_used;
Treelog::Open nest (msg, tmp_conv.str ());
if (debug == 7)
msg.touch ();
// Calculate conductivity - The Hansen method
for (size_t e = 0; e < edge_size; e++)
{
Ksum[e] += find_K_edge (soil, geo, e, h, h_ice, h_previous, T);
Kedge[e] = (Ksum[e] / (iterations_used + 0.0)+ Kold[e]) / 2.0;
}
//Initialize diffusive matrix
Solver::Matrix diff (cell_size);
// diff = ublas::zero_matrix<double> (cell_size, cell_size);
diffusion (geo, Kedge, diff);
//Initialize gravitational matrix
ublas::vector<double> grav (cell_size); //ublass compatibility
grav = ublas::zero_vector<double> (cell_size);
gravitation (geo, Kedge, grav);
// Boundary matrices and vectors
ublas::banded_matrix<double> Dm_mat (cell_size, cell_size,
0, 0); // Dir bc
Dm_mat = ublas::zero_matrix<double> (cell_size, cell_size);
ublas::vector<double> Dm_vec (cell_size); // Dir bc
Dm_vec = ublas::zero_vector<double> (cell_size);
ublas::vector<double> Gm (cell_size); // Dir bc
Gm = ublas::zero_vector<double> (cell_size);
ublas::vector<double> B (cell_size); // Neu bc
B = ublas::zero_vector<double> (cell_size);
lowerboundary (geo, groundwater, active_lysimeter, h,
Kedge,
dq, Dm_mat, Dm_vec, Gm, B, msg);
upperboundary (geo, soil, T, surface, state, remaining_water, h,
Kedge,
dq, Dm_mat, Dm_vec, Gm, B, ddt, debug, msg, dt);
Darcy (geo, Kedge, h, dq); //for calculating drain fluxes
//Initialize water capacity matrix
ublas::banded_matrix<double> Cw (cell_size, cell_size, 0, 0);
for (size_t c = 0; c < cell_size; c++)
Cw (c, c) = soil.Cw2 (c, h[c]);
std::vector<double> h_std (cell_size);
//ublas vector -> std vector
std::copy(h.begin (), h.end (), h_std.begin ());
#ifdef TEST_OM_DEN_ER_BRUGT
for (size_t cell = 0; cell != cell_size ; ++cell)
{
S_macro (cell) = (S_matrix[cell] + S_drain[cell])
* geo.cell_volume (cell);
}
#endif
//Initialize sum matrix
Solver::Matrix summat (cell_size);
noalias (summat) = diff + Dm_mat;
//Initialize sum vector
ublas::vector<double> sumvec (cell_size);
sumvec = grav + B + Gm + Dm_vec - S_vol
#ifdef TEST_OM_DEN_ER_BRUGT
- S_macro
#endif
;
// QCw is shorthand for Qmatrix * Cw
Solver::Matrix Q_Cw (cell_size);
noalias (Q_Cw) = prod (Qmat, Cw);
//Initialize A-matrix
Solver::Matrix A (cell_size);
noalias (A) = (1.0 / ddt) * Q_Cw - summat;
// Q_Cw_h is shorthand for Qmatrix * Cw * h
const ublas::vector<double> Q_Cw_h = prod (Q_Cw, h);
//Initialize b-vector
ublas::vector<double> b (cell_size);
//b = sumvec + (1.0 / ddt) * (Qmatrix * Cw * h + Qmatrix *(Wxx-Wyy));
b = sumvec + (1.0 / ddt) * (Q_Cw_h
+ prod (Qmat, Theta_previous-Theta));
// Force active drains to zero h.
drain (geo, drain_cell, drain_water_level,
h, Theta_previous, Theta, S_vol,
#ifdef TEST_OM_DEN_ER_BRUGT
S_macro,
#endif
dq, ddt, drain_cell_on, A, b, debug, msg);
try {
solver->solve (A, b, h); // Solve Ah=b with regard to h.
} catch (const char *const error) {
std::ostringstream tmp;
tmp << "Could not solve equation system: " << error;
msg.warning (tmp.str ());
// Try smaller timestep.
iterations_used = max_loop_iter + 100;
break;
}
for (int c=0; c < cell_size; c++) // update Theta
Theta (c) = soil.Theta (c, h (c), h_ice (c));
if (debug > 1)
{
std::ostringstream tmp;
tmp << "Time left = " << time_left << ", ddt = " << ddt
<< ", iteration = " << iterations_used << "\n";
tmp << "B = " << B << "\n";
tmp << "h = " << h << "\n";
tmp << "Theta = " << Theta;
msg.message (tmp.str ());
}
for (int c=0; c < cell_size; c++)
{
if (h (c) < min_pressure_potential || h (c) > max_pressure_potential)
{
std::ostringstream tmp;
tmp << "Pressure potential out of realistic range, h["
<< c << "] = " << h (c);
msg.debug (tmp.str ());
iterations_used = max_loop_iter + 100;
break;
}
}
}
while (!converges (h_conv, h) && iterations_used <= max_loop_iter);
if (iterations_used > max_loop_iter)
{
number_of_time_step_reductions++;
if (number_of_time_step_reductions > max_time_step_reductions)
{
msg.debug ("Could not find solution");
throw "Could not find solution";
}
iterations_with_this_time_step = 0;
ddt /= time_step_reduction;
h = h_previous;
Theta = Theta_previous;
}
else
{
// Update dq for new h.
ublas::banded_matrix<double> Dm_mat (cell_size, cell_size,
0, 0); // Dir bc
Dm_mat = ublas::zero_matrix<double> (cell_size, cell_size);
ublas::vector<double> Dm_vec (cell_size); // Dir bc
Dm_vec = ublas::zero_vector<double> (cell_size);
ublas::vector<double> Gm (cell_size); // Dir bc
Gm = ublas::zero_vector<double> (cell_size);
ublas::vector<double> B (cell_size); // Neu bc
B = ublas::zero_vector<double> (cell_size);
lowerboundary (geo, groundwater, active_lysimeter, h,
Kedge,
dq, Dm_mat, Dm_vec, Gm, B, msg);
upperboundary (geo, soil, T, surface, state, remaining_water, h,
Kedge,
dq, Dm_mat, Dm_vec, Gm, B, ddt, debug, msg, dt);
Darcy (geo, Kedge, h, dq);
#ifdef TEST_OM_DEN_ER_BRUGT
// update macropore flow components
for (int c = 0; c < cell_size; c++)
{
S_drain_sum[c] += S_drain[c] * ddt/dt;
S_matrix_sum[c] += S_matrix[c] * ddt/dt;
}
#endif
std::vector<double> h_std_new (cell_size);
std::copy(h.begin (), h.end (), h_std_new.begin ());
// Update remaining_water.
for (size_t i = 0; i < edge_above.size (); i++)
{
const int edge = edge_above[i];
const int cell = geo.edge_other (edge, Geometry::cell_above);
const double out_sign = (cell == geo.edge_from (edge))
? 1.0 : -1.0;
remaining_water[i] += out_sign * dq (edge) * ddt;
daisy_assert (std::isfinite (dq (edge)));
}
if (debug == 5)
{
std::ostringstream tmp;
tmp << "Remaining water at end: " << remaining_water;
msg.message (tmp.str ());
}
// Contribution to large time step.
daisy_assert (std::isnormal (dt));
daisy_assert (std::isnormal (ddt));
q += dq * ddt / dt;
for (size_t e = 0; e < edge_size; e++)
{
daisy_assert (std::isfinite (dq (e)));
daisy_assert (std::isfinite (q (e)));
}
for (size_t e = 0; e < edge_size; e++)
{
daisy_assert (std::isfinite (dq (e)));
daisy_assert (std::isfinite (q (e)));
}
time_left -= ddt;
iterations_with_this_time_step++;
if (iterations_with_this_time_step > time_step_reduction)
{
number_of_time_step_reductions--;
iterations_with_this_time_step = 0;
ddt *= time_step_reduction;
}
}
// End of small time step.
}
// Mass balance.
// New = Old - S * dt + q_in * dt - q_out * dt + Error =>
// 0 = Old - New - S * dt + q_in * dt - q_out * dt + Error
Theta_error -= Theta; // Old - New
Theta_error -= S * dt;
#ifdef TEST_OM_DEN_ER_BRUGT
for (size_t c = 0; c < cell_size; c++)
Theta_error (c) -= (S_matrix_sum[c] + S_drain_sum[c]) * dt;
#endif
for (size_t edge = 0; edge != edge_size; ++edge)
{
const int from = geo.edge_from (edge);
const int to = geo.edge_to (edge);
const double flux = q (edge) * geo.edge_area (edge) * dt;
if (geo.cell_is_internal (from))
Theta_error (from) -= flux / geo.cell_volume (from);
if (geo.cell_is_internal (to))
Theta_error (to) += flux / geo.cell_volume (to);
}
// Find drain sink from mass balance.
#ifdef TEST_OM_DEN_ER_BRUGT
std::fill(S_drain.begin (), S_drain.end (), 0.0);
#else
std::vector<double> S_drain (cell_size);
#endif
for (size_t i = 0; i < drain_cell.size (); i++)
{
const size_t cell = drain_cell[i];
S_drain[cell] = Theta_error (cell) / dt;
Theta_error (cell) -= S_drain[cell] * dt;
}
if (debug == 2)
{
double total_error = 0.0;
double total_abs_error = 0.0;
double max_error = 0.0;
int max_cell = -1;
for (size_t cell = 0; cell != cell_size; ++cell)
{
const double volume = geo.cell_volume (cell);
const double error = Theta_error (cell);
total_error += volume * error;
total_abs_error += std::fabs (volume * error);
if (std::fabs (error) > std::fabs (max_error))
{
max_error = error;
max_cell = cell;
}
}
std::ostringstream tmp;
tmp << "Total error = " << total_error << " [cm^3], abs = "
<< total_abs_error << " [cm^3], max = " << max_error << " [] in cell "
<< max_cell;
msg.message (tmp.str ());
}
// Make it official.
for (size_t cell = 0; cell != cell_size; ++cell)
soil_water.set_content (cell, h (cell), Theta (cell));
#ifdef TEST_OM_DEN_ER_BRUGT
soil_water.add_tertiary_sink (S_matrix_sum);
soil_water.drain (S_drain_sum, msg);
#endif
for (size_t edge = 0; edge != edge_size; ++edge)
{
daisy_assert (std::isfinite (q[edge]));
soil_water.set_flux (edge, q[edge]);
}
soil_water.drain (S_drain, msg);
// End of large time step.
}
bool
JacobianInverseKinematics::solve()
{
::std::chrono::steady_clock::time_point start = ::std::chrono::steady_clock::now();
double remaining = ::std::chrono::duration<double>(this->getDuration()).count();
::std::size_t iteration = 0;
::rl::math::Vector q = this->kinematic->getPosition();
::rl::math::Vector q2(this->kinematic->getDofPosition());
::rl::math::Vector dq(this->kinematic->getDofPosition());
::rl::math::Vector dx(6 * this->kinematic->getOperationalDof());
::rl::math::Vector rand(this->kinematic->getDof());
do
{
do
{
this->kinematic->forwardPosition();
dx.setZero();
for (::std::size_t i = 0; i < this->goals.size(); ++i)
{
::rl::math::VectorBlock dxi = dx.segment(6 * this->goals[i].second, 6);
dxi = this->kinematic->getOperationalPosition(this->goals[i].second).toDelta(this->goals[i].first);
}
if (dx.squaredNorm() < ::std::pow(this->getEpsilon(), 2))
{
this->kinematic->normalize(q);
this->kinematic->setPosition(q);
if (this->kinematic->isValid(q))
{
return true;
}
}
this->kinematic->calculateJacobian();
switch (this->method)
{
case METHOD_DLS:
this->kinematic->calculateJacobianInverse(0, false);
dq = this->kinematic->getJacobianInverse() * dx;
break;
case METHOD_SVD:
this->kinematic->calculateJacobianInverse(0, true);
dq = this->kinematic->getJacobianInverse() * dx;
break;
case METHOD_TRANSPOSE:
{
::rl::math::Vector tmp = this->kinematic->getJacobian() * this->kinematic->getJacobian().transpose() * dx;
::rl::math::Real alpha = dx.dot(tmp) / tmp.dot(tmp);
dq = alpha * this->kinematic->getJacobian().transpose() * dx;
}
break;
default:
break;
}
this->kinematic->step(q, dq, q2);
if (this->kinematic->transformedDistance(q, q2) > ::std::pow(this->delta, 2))
{
this->kinematic->interpolate(q, q2, this->delta, q2);
}
q = q2;
this->kinematic->setPosition(q);
remaining = ::std::chrono::duration<double>(this->getDuration() - (::std::chrono::steady_clock::now() - start)).count();
if (0 == ++iteration % 100)
{
break;
}
}
while (remaining > 0 && iteration < this->getIterations());
for (::std::size_t i = 0; i < this->kinematic->getDof(); ++i)
{
rand(i) = this->randDistribution(this->randEngine);
}
q = this->kinematic->generatePositionUniform(rand);
this->kinematic->setPosition(q);
remaining = ::std::chrono::duration<double>(this->getDuration() - (::std::chrono::steady_clock::now() - start)).count();
}
while (remaining > 0 && iteration < this->getIterations());
return false;
}
/*
* Do a multi-wait on the specified events.
*/
WORD ev_multi(WORD flags, MOBLK *pmo1, MOBLK *pmo2, LONG tmcount,
LONG buparm, LONG mebuff, WORD prets[])
{
QPB m;
register EVSPEC which;
register WORD what;
register CQUEUE *pc;
/* say nothing has */
/* happened yet */
what = 0x0;
which = 0;
/* do a pre-check for a */
/* keystroke & then */
/* clear out the forkq*/
chkkbd();
forker();
/* a keystroke */
if (flags & MU_KEYBD)
{
/* if a character is */
/* ready then get it */
pc = &rlr->p_cda->c_q;
if ( pc->c_cnt )
{
prets[4] = (UWORD) dq(pc);
what |= MU_KEYBD;
}
}
/* if we own the mouse */
/* then do quick chks */
if ( rlr == gl_mowner )
{
/* quick check button */
if (flags & MU_BUTTON)
{
if ( (mtrans > 1) &&
(downorup(pr_button, buparm)) )
{
what |= MU_BUTTON;
prets[5] = pr_mclick;
}
else
{
if ( downorup(button, buparm) )
{
what |= MU_BUTTON;
prets[5] = mclick;
}
}
}
/* quick check mouse rec*/
if ( ( flags & MU_M1 ) &&
( in_mrect(pmo1) ) )
what |= MU_M1;
/* quick check mouse rec*/
if ( ( flags & MU_M2 ) &&
( in_mrect(pmo2) ) )
what |= MU_M2;
}
/* quick check timer */
if (flags & MU_TIMER)
{
if ( tmcount == 0x0L )
what |= MU_TIMER;
}
/* quick check message */
if (flags & MU_MESAG)
{
if ( rlr->p_qindex > 0 )
{
ap_rdwr(MU_MESAG, rlr, 16, mebuff);
what |= MU_MESAG;
}
}
/* check for quick out */
/* if something has */
/* already happened */
if (what == 0x0)
{
/* wait for a keystroke */
if (flags & MU_KEYBD)
iasync( MU_KEYBD, 0x0L );
/* wait for a button */
if (flags & MU_BUTTON)
iasync( MU_BUTTON, buparm );
/* wait for mouse rect. */
if (flags & MU_M1)
iasync( MU_M1, ADDR(pmo1) );
/* wait for mouse rect. */
if (flags & MU_M2)
iasync( MU_M2, ADDR(pmo2) );
/* wait for message */
if (flags & MU_MESAG)
{
m.qpb_ppd = rlr;
m.qpb_cnt = 16;
m.qpb_buf = mebuff;
iasync( MU_MESAG, ADDR(&m) );
}
/* wait for timer */
if (flags & MU_TIMER)
iasync( MU_TIMER, tmcount / gl_ticktime );
/* wait for events */
which = mwait( flags );
/* cancel outstanding */
/* events */
which |= acancel( flags );
}
/* get the returns */
ev_rets(&prets[0]);
/* do aprets() if */
/* something hasn't */
/* already happened */
if (what == 0x0)
{
what = which;
if (which & MU_KEYBD)
prets[4] = apret(MU_KEYBD);
if (which & MU_BUTTON)
prets[5] = apret(MU_BUTTON);
if (which & MU_M1)
apret(MU_M1);
if (which & MU_M2)
apret(MU_M2);
if (which & MU_MESAG)
apret(MU_MESAG);
if (which & MU_TIMER)
apret(MU_TIMER);
}
/* return what happened*/
return( what );
}
ll solve(){
return 1LL * N * (N - 1) / 2 - dq(1);
}
/*
* Flush the characters from a circular keyboard buffer
*/
void fq(void)
{
while (rlr->p_cda->c_q.c_cnt)
dq(&rlr->p_cda->c_q);
}
void SeniorVMHandle::q_push_imm(long n)
{
db(HANDLE.q_push_imm.handle);
dq(n);
}
dgFloat32 dgCollisionChamferCylinder::RayCast(const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
if (q0.m_x > m_height) {
if (q1.m_x < m_height) {
dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
//contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
if (q0.m_x < -m_height) {
if (q1.m_x > -m_height) {
dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
//contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
dgVector dq((q1 - q0) & dgVector::m_triplexMask);
// avoid NaN as a result of a division by zero
if ((dq % dq) <= 0.0f) {
return dgFloat32(1.2f);
}
dgVector dir(dq.CompProduct4(dq.InvMagSqrt()));
if (dgAbsf(dir.m_x) > 0.9999f) {
return dgCollisionConvex::RayCast(q0, q1, maxT, contactOut, NULL, NULL, NULL);
}
dgVector q1q0 (q1 - q0);
dgFloat32 t = -q0.m_x / q1q0.m_x;
dgVector p0(q0 + q1q0.Scale4(t));
if ((p0 % p0) < (m_radius + m_height)) {
dgVector origin0(p0.CompProduct4(p0.InvMagSqrt()).Scale4(m_radius));
dgVector origin1(origin0.CompProduct4(dgVector::m_negOne));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
dgFloat32 t1 = dgRayCastSphere(q0, q1, origin1, m_height);
if(t1 < t0) {
t0 = t1;
origin0 = origin1;
}
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale4(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
contactOut.m_normal = contactOut.m_normal.CompProduct4(contactOut.m_normal.DotProduct4(contactOut.m_normal).InvSqrt());
return t0;
} else {
q1q0.m_x = dgFloat32(0.0f);
q1q0 = q1q0.CompProduct4(q1q0.InvMagSqrt());
origin0 = q1q0.Scale4(-m_radius);
t0 = dgRayCastSphere(q0, q1, origin0, m_height);
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale4(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
contactOut.m_normal = contactOut.m_normal.CompProduct4(contactOut.m_normal.DotProduct4(contactOut.m_normal).InvSqrt());
return t0;
}
}
}
return dgFloat32(1.2f);
}
