bool auto_move::calculate_effect(HIP& hip, double* out_force)
{
// 1cm/sec at a depth of 1cm. == 1m/sec at 1m
const static double velocity_scale = 1; // units 1 (cm/sec)/cm = 1/sec
bool contact = false;
btVector3 pos_incr;
for (int i=0;i<3;i++)
out_force[i]=0;
// Calculate desired velocity
// Velocity command is a function of penetration depth into the feature
btVector3 velocity = normalized_direction_to_target(hip) * velocity_scale * penetration_depth(hip); // units: m/sec
// Calculate a position increment1
// pos_incr = timestep * vel_vector = t * d/t
if (hip.get_last_time_incr() < TENTH_SECOND)
{
pos_incr = hip.get_last_time_incr() * velocity;
}
// Send position increment to robot
publish_automove(pos_incr, hip.id());
return contact;
}
/***
*
* Assistive features
* jestream--- auto move with velocity proportional to height
*
* ***/
bool jetstream::calculate_effect(HIP& hip, double* out_force)
{
// 1 = 1cm/sec at a depth of 1cm. == 1m/sec at 1m
const static double velocity_scale = 2; // units 1 (cm/sec)/cm = 1/sec
btVector3 pos_incr;
for (int i=0;i<3;i++)
out_force[i]=0;
bool contact = is_in_contact(hip);
if ( contact && distance_to_target(hip) > target_radius)
{
// Calculate desired velocity
// Velocity command is a function of penetration depth into the feature
btVector3 velocity = normalized_direction_to_target(hip) * velocity_scale * sqrt(penetration_depth(hip)); // units: m/sec
// Calculate a position increment1
if (hip.get_last_time_incr() < TENTH_SECOND)
{
pos_incr = hip.get_last_time_incr() * velocity;
}
// Send position increment to robot
publish_automove(pos_incr, hip.id());
}
return contact;
}
bool penetration_depth(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin,
const DT_Convex& b, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb)
{
DT_HybridPack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), b, b_margin);
MT_Scalar max_pen_len = MT_Scalar(0.0);
return penetration_depth(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), pack, v, pa, pb, max_pen_len);
}
bool jetstream::is_in_contact(HIP& hip)
{
return ( penetration_depth(hip) != 0 );
}
DT_Bool DT_Encounter::exactTest(const DT_RespTable *respTable, int& count) const
{
const DT_ResponseList& responseList = respTable->find(m_obj_ptr1, m_obj_ptr2);
switch (responseList.getType())
{
case DT_SIMPLE_RESPONSE:
if (intersect(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis))
{
++count;
return (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2)) ?
responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), 0) :
responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), 0);
}
break;
case DT_WITNESSED_RESPONSE: {
MT_Point3 p1, p2;
if (common_point(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis, p1, p2))
{
++count;
if (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2))
{
DT_CollData coll_data;
p1.getValue(coll_data.point1);
p2.getValue(coll_data.point2);
return responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), &coll_data);
}
else
{
DT_CollData coll_data;
p1.getValue(coll_data.point2);
p2.getValue(coll_data.point1);
return responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), &coll_data);
}
}
break;
}
case DT_DEPTH_RESPONSE: {
MT_Point3 p1, p2;
if (penetration_depth(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis, p1, p2))
{
++count;
if (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2))
{
DT_CollData coll_data;
p1.getValue(coll_data.point1);
p2.getValue(coll_data.point2);
(p2 - p1).getValue(coll_data.normal);
return responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), &coll_data);
}
else
{
DT_CollData coll_data;
p1.getValue(coll_data.point2);
p2.getValue(coll_data.point1);
(p1 - p2).getValue(coll_data.normal);
return responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), &coll_data);
}
}
break;
}
case DT_NO_RESPONSE:
break;
default:
assert(false);
}
return DT_CONTINUE;
}
