/* Helper function that sets the joint's base position
* This places the kinematic chain at the appropriate position around the base of the hand.
*
* @param h - Pointer to the hand being created
* @param fingerNumber - The index of the kinematic chain being added.
*/
void EigenHandLoader::setBaseJointPosition(Hand * h, unsigned int fingerNumber)
{
// Get base joint
Joint * j = h->getChain(fingerNumber)->getJoint(0);
// Calculate offset angle from database joint description
double offsetAngle = hd->fingerBasePositions[fingerNumber]*M_PI/180.0;
// Set the offset angle of the joint
j->setOffset(offsetAngle);
// Disable collisions between the palm and the first link.
h->getWorld()->toggleCollisions(false, h->getChain(fingerNumber)->getLink(1), h->getPalm());
// These calculations are probably obsolete.
//j->setMin(j->getMin()+offsetAngle);
//if(j->getMin() > M_PI)
// j->setMin(j->getMin() - 2*M_PI);
//j->setMax(j->getMax() + offsetAngle);
//if(j->getMax() > M_PI)
// j->setMax(j->getMax() - 2*M_PI);
// Set the current joint position to 0.0, given the new offset
j->setVal(0.0);
}
void BodyNode::updateBodyForce(const Eigen::Vector3d& _gravity,
bool _withExternalForces) {
if (mGravityMode == true)
mFgravity.noalias() = mI * math::AdInvRLinear(mW, _gravity);
else
mFgravity.setZero();
mF.noalias() = mI * mdV; // Inertial force
if (_withExternalForces)
mF -= mFext; // External force
for (int i = 0; i < mContactForces.size(); ++i)
mF -= mContactForces[i];
assert(!math::isNan(mF));
mF -= mFgravity; // Gravity force
mF -= math::dad(mV, mI * mV); // Coriolis force
for (std::vector<BodyNode*>::iterator iChildBody = mChildBodyNodes.begin();
iChildBody != mChildBodyNodes.end(); ++iChildBody) {
Joint* childJoint = (*iChildBody)->getParentJoint();
assert(childJoint != NULL);
mF += math::dAdInvT(childJoint->getLocalTransform(),
(*iChildBody)->getBodyForce());
}
assert(!math::isNan(mF));
}
//Create new joint and place it in the middle of screen
void DrawingWindow::createJoint()
{
Joint *joint = new Joint;
joint->setPos(500,500);
joints << joint;
drawingScene->addItem(joint);
}
shared_ptr<Joint> Joint::GetJoint(dJointID id)
{
if (id == 0)
{
return shared_ptr<Joint>();
}
Joint* jointPtr =
static_cast<Joint*>(dJointGetData(id));
if (jointPtr == 0)
{
// we cannot use the logserver here
cerr << "ERROR: (Joint) no joint found for dJointID "
<< id << "\n";
return shared_ptr<Joint>();
}
shared_ptr<Joint> joint = shared_static_cast<Joint>
(jointPtr->GetSelf().lock());
if (joint.get() == 0)
{
// we cannot use the logserver here
cerr << "ERROR: (Joint) got no shared_ptr for dJointID "
<< id << "\n";
}
return joint;
}
boost::shared_ptr<Joint> Joint::GetJoint(long jointID)
{
if (jointID == 0)
{
return boost::shared_ptr<Joint>();
}
Joint* jointPtr = mJointImp->GetJoint(jointID);
if (jointPtr == 0)
{
// we cannot use the logserver here
cerr << "ERROR: (Joint) no joint found for dJointID "
<< jointID << "\n";
return boost::shared_ptr<Joint>();
}
boost::shared_ptr<Joint> joint = static_pointer_cast<Joint>
(jointPtr->GetSelf().lock());
if (joint.get() == 0)
{
// we cannot use the logserver here
cerr << "ERROR: (Joint) got no boost::shared_ptr for dJointID "
<< jointID << "\n";
}
return joint;
}
/**
HACK!
*/
void Character::updateJointOrdering(){
if( jointOrder.size() == joints.size() )
return; // HACK assume ordering is ok
jointOrder.clear();
if (!root)
return;
DynamicArray<ArticulatedRigidBody*> bodies;
bodies.push_back(root);
int currentBody = 0;
while ((uint)currentBody<bodies.size()){
//add all the children joints to the list
for (int i=0;i<bodies[currentBody]->getChildJointCount();i++){
Joint *j = bodies[currentBody]->getChildJoint(i);
jointOrder.push_back( getJointIndex(j->getName()) );
bodies.push_back(bodies[currentBody]->getChildJoint(i)->getChild());
}
currentBody++;
}
reverseJointOrder.resize( jointOrder.size() );
for( uint i=0; i < jointOrder.size(); ++i )
reverseJointOrder[jointOrder[i]] = i;
}
Json::Value FileSaver::StoreSkeleton(const SkeletonData& skeleton)
{
Json::Value value;
const std::map<ee::SprPtr, std::vector<Joint*> >& map_joints = skeleton.GetMapJoints();
std::map<ee::SprPtr, std::vector<Joint*> >::const_iterator itr
= map_joints.begin();
for (int i = 0; itr != map_joints.end(); ++itr, ++i)
{
value[i]["sprite"] = itr->first->GetName();
for (int j = 0, m = itr->second.size(); j < m; ++j)
{
Joint* joint = itr->second[j];
Json::Value joint_val;
joint_val["id"] = joint->GetID();
joint_val["rx"] = joint->GetCurrRelativePos().x;
joint_val["ry"] = joint->GetCurrRelativePos().y;
if (const Joint* parent = joint->GetParent()) {
joint_val["parent"] = parent->GetID();
}
const std::set<Joint*>& children = joint->GetChildren();
std::set<Joint*>::const_iterator itr_child = children.begin();
for (int k = 0; itr_child != children.end(); ++itr_child, ++k) {
joint_val["children"][k] = (*itr_child)->GetID();
}
value[i]["joints"][j] = joint_val;
}
}
return value;
}
//==============================================================================
Eigen::Vector3d State::_getJointPosition(BodyNode* _bodyNode) const
{
Joint* parentJoint = _bodyNode->getParentJoint();
Eigen::Vector3d localJointPosition
= parentJoint->getTransformFromChildBodyNode().translation();
return _bodyNode->getTransform() * localJointPosition;
}
/**
This method makes it possible to evaluate the debug pose at any phase angle
*/
void SimBiController::updateTrackingPose(DynamicArray<double>& trackingPose, double phiToUse){
if( phiToUse < 0 )
phiToUse = phi;
if( phiToUse > 1 )
phiToUse = 1;
trackingPose.clear();
this->character->getState(&trackingPose);
ReducedCharacterState debugRS(&trackingPose);
//always start from a neutral desired pose, and build from there...
for (int i=0;i<jointCount;i++){
debugRS.setJointRelativeOrientation(Quaternion(1, 0, 0, 0), i);
debugRS.setJointRelativeAngVelocity(Vector3d(), i);
controlParams[i].relToCharFrame = false;
}
//and this is the desired orientation for the root
Quaternion qRootD(1, 0, 0, 0);
SimBiConState* curState = states[FSMStateIndex];
for (int i=0;i<curState->getTrajectoryCount();i++){
//now we have the desired rotation angle and axis, so we need to see which joint this is intended for
int jIndex = curState->sTraj[i]->getJointIndex(stance);
//if the index is -1, it must mean it's the root's trajectory. Otherwise we should give an error
if (curState->sTraj[i]->relToCharFrame == true || jIndex == swingHipIndex)
controlParams[jIndex].relToCharFrame = true;
Vector3d d0, v0;
computeD0( phiToUse, &d0 );
computeV0( phiToUse, &v0 );
Quaternion newOrientation = curState->sTraj[i]->evaluateTrajectory(this, character->getJoint(jIndex), stance, phiToUse, d - d0, v - v0);
if (jIndex == -1){
qRootD = newOrientation;
}else{
debugRS.setJointRelativeOrientation(newOrientation, jIndex);
}
}
debugRS.setOrientation(qRootD);
//now, we'll make one more pass, and make sure that the orientations that are relative to the character frame are drawn that way
for (int i=0;i<jointCount;i++){
if (controlParams[i].relToCharFrame){
Quaternion temp;
Joint* j = character->getJoint(i);
ArticulatedRigidBody* parent = j->getParent();
while (parent != root){
j = parent->getParentJoint();
parent = j->getParent();
temp = debugRS.getJointRelativeOrientation(character->getJointIndex(j->name)) * temp;
}
temp = qRootD * temp;
temp = temp.getComplexConjugate() * debugRS.getJointRelativeOrientation(i);
debugRS.setJointRelativeOrientation(temp, i);
}
}
}
void CollisionSpace::drawCollisionPoints() {
for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) {
Joint* jnt = m_Robot->getJoint(i);
Eigen::Transform3d T = jnt->getMatrixPos();
std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt);
for (unsigned int j = 0; j < points.size(); j++) {
// if (points[j].getSegmentNumber() <ENV.getDouble(Env::extensionStep))
// continue;
if (points[j].m_is_colliding) {
double color[4];
color[1] = 0.0; // green
color[2] = 0.0; // blue
color[0] = 1.0; // red
color[3] = 0.7; // transparency
g3d_set_color(Any, color);
}
bool yellow = (!points[j].m_is_colliding);
points[j].draw(T, yellow);
}
}
}
double CollisionSpace::cost(const Configuration& q) const {
// bool colliding = false;
double distance = 0.0;
double potential = 0.0;
// colliding =
// isRobotColliding( distance, potential );
double cost = 0.0;
Eigen::Vector3d position, gradient;
for (unsigned int i = 0; i < m_Robot->getNumberOfJoints(); i++) {
Joint* jnt = m_Robot->getJoint(i);
std::vector<CollisionPoint>& points = m_sampler->getCollisionPoints(jnt);
if (points.empty()) continue;
Eigen::Transform3d T = jnt->getMatrixPos();
for (unsigned int j = 0; j < points.size(); j++) {
position = T * points[j].getPosition();
getCollisionPointPotentialGradient(
points[j], position, distance, potential, gradient);
if (potential < EPS6) {
potential = EPS6;
}
cost += potential;
}
}
return 10 * cost;
}
void check_self_consistency(SkeletonPtr skeleton)
{
for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i)
{
BodyNode* bn = skeleton->getBodyNode(i);
EXPECT_TRUE(bn->getIndexInSkeleton() == i);
EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn);
Joint* joint = bn->getParentJoint();
EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint);
for(size_t j=0; j<joint->getNumDofs(); ++j)
{
DegreeOfFreedom* dof = joint->getDof(j);
EXPECT_TRUE(dof->getIndexInJoint() == j);
EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
}
}
for(size_t i=0; i<skeleton->getNumDofs(); ++i)
{
DegreeOfFreedom* dof = skeleton->getDof(i);
EXPECT_TRUE(dof->getIndexInSkeleton() == i);
EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
}
}
void Boxes::drawCollisionPoints()
{
for( size_t i=0; i<active_joints_.size(); i++ )
{
Joint* jnt = robot_->getJoint( active_joints_[i] );
std::vector<CollisionPoint>& points = sampler_->getCollisionPoints(jnt);
Eigen::Transform3d T = jnt->getMatrixPos();
cout << "joint : " << active_joints_[i] << " , points size : " << points.size() << endl;
for( size_t j=0; j<points.size(); j++ )
{
if( points[j].m_is_colliding )
{
double color[4];
color[0] = 1.0; // (r)ed
color[1] = 0.0; // (g)reen
color[2] = 0.0; // (b)lue
color[3] = 0.7; // transparency
g3d_set_color(Any,color);
}
bool yellow = true;
//bool yellow = (!points[j].m_is_colliding);
points[j].draw( T, yellow );
}
}
}
int w_Joint_getReactionTorque(lua_State *L)
{
Joint *t = luax_checkjoint(L, 1);
float inv_dt = (float)luaL_checknumber(L, 2);
lua_pushnumber(L, t->getReactionTorque(inv_dt));
return 1;
}
//todo плохо что надо учитывать interact
void PhysLevelObject::requestDelete(bool instant) {
for(auto& iter : destroyListeners) {
iter();
}
// Очень странно, ну а вдруг
if(body==nullptr) {
LevelObject::requestDelete(instant);
return;
}
// Родителький requestDelete вызываться не будет, поэтому делаем за него удаление interact-а
if(getInteract()) {
getInteract()->getLayer()->deleteObject(getInteract());
}
for(b2JointEdge* edge = getBody()->GetJointList(); edge; edge=edge->next) {
Joint* j = static_cast<Joint*>(edge->joint->GetUserData());
j->getLayer()->deleteObject(j);
}
Game::getPhysicsSystem()->deleteObject(body);
Game::getPhysicsSystem()->addPostStepAction([this]() {
delete this;
});
}
void Camera::processFrame()
{
this->capture.read(this->cameraImage);
for (unsigned int i = 0 ; i < this->objectsToTrack.size() ; i++)
{
cvtColor(this->cameraImage, this->HSVImage, COLOR_BGR2HSV);
inRange(this->HSVImage, this->objectsToTrack[i]->getHSVmin(),
this->objectsToTrack[i]->getHSVmax(), this->thresholdImage);
this->morphOps(this->thresholdImage);
this->trackFilteredObject(this->objectsToTrack[i], this->thresholdImage,
this->HSVImage, this->cameraImage);
if (this->head != NULL && i == this->focusObjIndex)
{
Joint *pan = head->getJoint(PAN);
Joint *tilt = head->getJoint(TILT);
float x = this->objectsToTrack[i]->getXPos();
float y = this->objectsToTrack[i]->getYPos();
float half_width = float(CAMERA_WIDTH) / 2.0;
float half_height = float(CAMERA_HEIGHT) / 2.0;
float pan_angle = ((x - half_width) / half_width)*(CAMERA_HORIZONTAL_FIELD/2.0);
float tilt_angle = ((y - half_height) / half_height)*(CAMERA_VERTICAL_FIELD/2.0);
this->focusPan = pan->getAngle() - pan_angle;
this->focusTilt = tilt->getAngle() - tilt_angle;
}
}
imshow("Dimitri's Vision", cameraImage);
}
void PoseIK::scalingJacobian( math::matrixN& J )
{
unsigned int i, j;
double c = 0.0f;
unsigned int jj = 0;
for( j=0; j < NUM_JOINTS_IN_HUMAN; j++ )
{
Joint* joint = skeleton->getHumanJoint( j );
if( joint )
{
unsigned int dof = joint->getDOF();
switch( dof )
{
case 6 :
c = PoseIK::RigidityCoeff[ j ];
for( i=0; i < num_equations; i++ )
{
J[i][jj ] *= c;
J[i][jj+1] *= c;
J[i][jj+2] *= c;
}
c = PoseIK::RigidityCoeff[ j ];
for( i=0; i < num_equations; i++ )
{
J[i][jj+3] *= c;
J[i][jj+4] *= c;
J[i][jj+5] *= c;
}
break;
case 3 :
c = PoseIK::RigidityCoeff[ j ];
for( i=0; i < num_equations; i++ )
{
J[i][jj ] *= c;
J[i][jj+1] *= c;
J[i][jj+2] *= c;
}
break;
case 1 :
c = PoseIK::RigidityCoeff[ j ];
for( i=0; i < num_equations; i++ )
{
J[i][jj ] *= c;
}
break;
default :
break;
}
jj += dof;
}
}
}
void Animation::AnimateSkeleton(Skeleton* targetSkeleton, unsigned int frameA, unsigned int frameB, float interpolation) const
{
#if NAZARA_UTILITY_SAFE
if (!m_impl)
{
NazaraError("Animation not created");
return;
}
if (m_impl->type != AnimationType_Skeletal)
{
NazaraError("Animation is not skeletal");
return;
}
if (!targetSkeleton || !targetSkeleton->IsValid())
{
NazaraError("Target skeleton is invalid");
return;
}
if (targetSkeleton->GetJointCount() != m_impl->jointCount)
{
NazaraError("Target skeleton joint count must match animation joint count");
return;
}
if (frameA >= m_impl->frameCount)
{
NazaraError("Frame A is out of range (" + String::Number(frameA) + " >= " + String::Number(m_impl->frameCount) + ')');
return;
}
if (frameB >= m_impl->frameCount)
{
NazaraError("Frame B is out of range (" + String::Number(frameB) + " >= " + String::Number(m_impl->frameCount) + ')');
return;
}
#endif
#ifdef NAZARA_DEBUG
if (interpolation < 0.f || interpolation > 1.f)
{
NazaraError("Interpolation must be in range [0..1] (Got " + String::Number(interpolation) + ')');
return;
}
#endif
for (unsigned int i = 0; i < m_impl->jointCount; ++i)
{
Joint* joint = targetSkeleton->GetJoint(i);
SequenceJoint& sequenceJointA = m_impl->sequenceJoints[frameA*m_impl->jointCount + i];
SequenceJoint& sequenceJointB = m_impl->sequenceJoints[frameB*m_impl->jointCount + i];
joint->SetPosition(Vector3f::Lerp(sequenceJointA.position, sequenceJointB.position, interpolation));
joint->SetRotation(Quaternionf::Slerp(sequenceJointA.rotation, sequenceJointB.rotation, interpolation));
joint->SetScale(Vector3f::Lerp(sequenceJointA.scale, sequenceJointB.scale, interpolation));
}
}
Control::Control(uint8_t id1, uint8_t id2, uint8_t id3)
{
jointTC.SetId(id1);
jointCT.SetId(id2);
jointFT.SetId(id3);
}
void CollisionSpace::addEnvPoints() {
// Add Static Obstacles
for (int i = 0; i < XYZ_ENV->no; i++) {
PointCloud& cloud = m_sampler->getPointCloud(XYZ_ENV->o[i]);
for (unsigned int j = 0; j < cloud.size(); j++)
m_points_to_add.push_back(cloud[j]);
}
// Add Moving Obstacles
Scene* sc = global_Project->getActiveScene();
for (unsigned int i = 0; i < sc->getNumberOfRobots(); i++) {
Robot* mov_obst = sc->getRobot(i);
// The robot is not a robot or a human
if (!((mov_obst->getName().find("ROBOT") != std::string::npos) ||
(mov_obst->getName().find("HUMAN") != std::string::npos) ||
mov_obst->getName() == "rob1" || mov_obst->getName() == "rob2" ||
mov_obst->getName() == "rob3" || mov_obst->getName() == "rob4")) {
cout << "Adding : " << mov_obst->getName() << endl;
Joint* jnt = mov_obst->getJoint(1);
if (static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o == NULL) continue;
PointCloud& cloud = m_sampler->getPointCloud(
static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o);
for (int j = 0; j < int(cloud.size()); j++) {
m_points_to_add.push_back(jnt->getMatrixPos() * cloud[j]);
}
}
}
}
void NiutHumanReader::updateJoint(int i, int j, Joint& curJoint, int toasterId, std::vector<int>& trackedJoints, const niut_msgs::niut_HUMAN_LIST::ConstPtr& msg) {
std::map<int, std::string> niutToString;
niutToString[0] = "HEAD";
niutToString[3] = "TORSO";
niutToString[9] = "L_HAND";
niutToString[15] = "R_HIP";
niutToString[22] = "R_HAND";
double x, y, z;
int niutJoint = trackedJoints[j];
std::string jointString = niutToString[niutJoint];
//Allocate Joint if needed
if (lastConfig_[toasterId]->skeleton_[jointString] == NULL) {
Joint* tmpJoint = new Joint(10001 + j, toasterId);
tmpJoint->setName(jointString);
tmpJoint->setAgentId(toasterId);
lastConfig_[toasterId]->skeleton_[jointString] = tmpJoint;
}
x = msg->filtered_users[i].skeleton.joint[niutJoint].position.x;
y = msg->filtered_users[i].skeleton.joint[niutJoint].position.y;
z = msg->filtered_users[i].skeleton.joint[niutJoint].position.z;
curJoint.position_.set<0>(x);
curJoint.position_.set<1>(y);
curJoint.position_.set<2>(z);
projectJoint(curJoint, kinectPos_);
}
void Joint::addChild(QTreeWidgetItem *child)
{
QTreeWidgetItem::addChild(child);
Joint* j = (Joint*)child;
children.push_back(j);
j->setParent(this);
}
Joint *luax_checkjoint(lua_State *L, int idx)
{
Joint *t = luax_checktype<Joint>(L, idx);
if (!t->isValid())
luaL_error(L, "Attempt to use destroyed joint.");
return t;
}
int w_Joint_getType(lua_State *L)
{
Joint *t = luax_checkjoint(L, 1);
const char *type = "";
Joint::getConstant(t->getType(), type);
lua_pushstring(L, type);
return 1;
}
Joint MakeJoint(vector<RoadPoint> const & points)
{
Joint joint;
for (auto const & point : points)
joint.AddPoint(point);
return joint;
}
void Canvas::load(string path) {
cout << "load" << endl;
for (int i = 0; i < lines.size(); i++) {
lines[i]->release();
delete lines[i];
}
lines.clear();
while (!joints.empty()) {
delete joints.back();
joints.pop_back();
}
int lines_n;
int n;
ifstream file(path.c_str());
if (file.is_open()) {
int i = 0;
file >> n;
for (int i = 0; i < n; i++) {
Joint *v = new Joint();
v->setupGui();
file >> v->p.x;
file >> v->p.y;
int fixed;
file >> fixed;
v->fixed = (bool)fixed;
*v->fixed_toggle = v->fixed;
v->joint_type = JOINT_REVOLUTE;
v->id = i;
joints.push_back(v);
}
file >> lines_n;
for (int i = 0; i < lines_n; i++) {
lines.push_back(new Polyline());
Polyline *l = lines.back();
l->id = lines.size() - 1;
file >> n;
file >> l->closed;
for (int i = 0; i < n; i++) {
Vertex v;
file >> v.p.x;
file >> v.p.y;
l->addBack(&v);
}
if (l->closed) {
l->back->next = l->front;
l->front->prev = l->back;
}
l->update();
}
file.close();
}
Joint* PhysicsSim::createJoint(PhysicalObject* obj1, PhysicalObject* obj2, Vector3D anchor, Vector3D axis)
{
Joint* jt = new Joint(obj1, obj2, anchor, axis);
dynamicsWorld->addConstraint(jt->getConstraint(), true);
joints_list.push_back(jt);
return jt;
}
bool GameObjectManager::initializeCharacterNodeScale(Node* node)
{
Joint* joint = dynamic_cast<Joint*>(node);
if (joint)
{
joint->setScale(CHARACTER_SCALE, CHARACTER_SCALE, CHARACTER_SCALE);
}
return true;
}
static void CreatJoint( const AcDbObjectId& host, const AcGePoint3d& pt, double angle )
{
Joint* pStation = new Joint( pt, angle );
// 关联到巷道上
pStation->setRelatedGE( host );
// 初始化并提交到数据库
if( !ArxUtilHelper::PostToModelSpace( pStation ) ) delete pStation;
}
const Joint& Robot::const_joint(size_t jointIndex) const
{
if(jointIndex < nJoints())
return *joints_[jointIndex];
Joint* invalidJoint = new Joint;
invalidJoint->name("invalid");
return *invalidJoint;
}
