C++ Body类代码示例

void DynamicBody::CalcExternalForce()
{
    // gravity
    if (!GetFrame()) return;			// no external force if not in a frame
    Body *body = GetFrame()->GetBody();
    if (body && !body->IsType(Object::SPACESTATION)) {	// they ought to have mass though...
        vector3d b1b2 = GetPosition();
        double m1m2 = GetMass() * body->GetMass();
        double invrsqr = 1.0 / b1b2.LengthSqr();
        double force = G*m1m2 * invrsqr;
        m_externalForce = -b1b2 * sqrt(invrsqr) * force;
    }
    else m_externalForce = vector3d(0.0);
    m_gravityForce = m_externalForce;

    // atmospheric drag
    if (body && GetFrame()->IsRotFrame() && body->IsType(Object::PLANET))
    {
        vector3d dragDir = -m_vel.NormalizedSafe();
        vector3d fDrag = CalcAtmosphericForce(m_dragCoeff)*dragDir;

        // make this a bit less daft at high time accel
        // only allow atmosForce to increase by .1g per frame
        vector3d f1g = m_atmosForce + dragDir * GetMass();
        if (fDrag.LengthSqr() > f1g.LengthSqr()) m_atmosForce = f1g;
        else m_atmosForce = fDrag;

        m_externalForce += m_atmosForce;
    }
    else m_atmosForce = vector3d(0.0);

    // centrifugal and coriolis forces for rotating frames
    if (GetFrame()->IsRotFrame()) {
        vector3d angRot(0, GetFrame()->GetAngSpeed(), 0);
        m_externalForce -= m_mass * angRot.Cross(angRot.Cross(GetPosition()));	// centrifugal
        m_externalForce -= 2 * m_mass * angRot.Cross(GetVelocity());			// coriolis
    }
}

void ObjectViewerView::OnChangeGeoSphereStyle()
{
	SBody sbody;

	const fixed volatileGas = fixed(65536.0*atof(m_sbodyVolatileGas->GetText().c_str()), 65536);
	const fixed volatileLiquid = fixed(65536.0*atof(m_sbodyVolatileLiquid->GetText().c_str()), 65536);
	const fixed volatileIces = fixed(65536.0*atof(m_sbodyVolatileIces->GetText().c_str()), 65536);
	const fixed life = fixed(65536.0*atof(m_sbodyLife->GetText().c_str()), 65536);
	const fixed volcanicity = fixed(65536.0*atof(m_sbodyVolcanicity->GetText().c_str()), 65536);
	const fixed metallicity = fixed(65536.0*atof(m_sbodyMetallicity->GetText().c_str()), 65536);
	const fixed mass = fixed(65536.0*atof(m_sbodyMass->GetText().c_str()), 65536);
	const fixed radius = fixed(65536.0*atof(m_sbodyRadius->GetText().c_str()), 65536);

	sbody.parent = 0;
	sbody.name = "Test";
	/* These should be the only SBody attributes GeoSphereStyle uses */
	sbody.type = SBody::TYPE_PLANET_TERRESTRIAL;
	sbody.seed = atoi(m_sbodySeed->GetText().c_str());
	sbody.radius = radius;
	sbody.mass = mass;
	sbody.averageTemp = 273;
	sbody.m_metallicity = metallicity;
	sbody.m_volatileGas = volatileGas;
	sbody.m_volatileLiquid = volatileLiquid;
	sbody.m_volatileIces = volatileIces;
	sbody.m_volcanicity = volcanicity;
	sbody.m_life = life;
	sbody.heightMapFilename = 0;

	Body *body = Pi::player->GetNavTarget();
	if (body->IsType(Object::PLANET)) {
		Planet *planet = static_cast<Planet*>(body);
		GeoSphere *gs = planet->GetGeoSphere();
		gs->m_style = GeoSphereStyle(&sbody);
		// force rebuild
		gs->OnChangeDetailLevel();
	}
}

Entity* CreateFloor(SimpleTree& _rParentNode)
{
	Entity* pFloor = new Entity;

	// The model
	Plane* pPlane = new Plane(Vector3(0.0f, 1.0f, 0.0f), FLOOR_POSITION);
	pFloor->addComponent(pPlane);
	pPlane->setEntity(pFloor);

	// The visible model
	Cube* pVisibleFloor = new Cube(Vector2(), 500.0f);
	pVisibleFloor->setColour(Vector4(0.0f, 0.5f, 0.0f, 1.0f));
	pFloor->addComponent(pVisibleFloor);
	pVisibleFloor->setEntity(pFloor);

	// The physical body
	Body::Material material;
	material.density = 0.5f;
	material.friction = 0.5f;
	material.restitution = 0.5f;
	Body* pBody = PhysicsFactory::getInstance()->createBody(material, pPlane, FLOOR_POSITION, false);
	pFloor->addComponent(pBody);
	pBody->setEntity(pFloor);

	// The scene
	SimpleTree* pNode = new SimpleTree;
	pNode->setModel(pPlane);
	pBody->setNode(pNode);
	_rParentNode.addChild(pNode);

	SimpleTree* pVisibleNode = new SimpleTree;
	getTranslation3(pVisibleNode->getTransformation()) = FLOOR_POSITION + Vector3(0.0f, -500.0f, -500.0f);
	pVisibleNode->setModel(pVisibleFloor);
	_rParentNode.addChild(pVisibleNode);

	GazEngine::addEntity(pFloor);
	return pFloor;
}

bool EditTool::onMousePress( const QPointF& pos_ ) {
    m_selected = scene()->selected();
    Body* b = scene()->activeBody();
    QPointF pos = pos_;
    if (m_selected) {
        if (b) {
            pos = b->toLocal(pos_);
        }
        PrimitiveMarker* marker = m_selected->getMarkerAtPoint(pos);
        if (marker) {
            m_start_pos = marker->position();
            m_offset = marker->getOffset( pos );
            m_selected_marker = marker;
            m_selected_marker->activate();
            m_moved = false;
            return true;
        }
    }
    m_selected_marker = 0;
    pos = pos_;
    Primitive* primitive = scene()->getPrimitiveAtPoint( pos );

    if (primitive) {
        scene()->setSelected( primitive );
        b = scene()->activeBody();
        if (b) {
            pos = b->toLocal(pos_);
        }
        m_start_pos = primitive->position();
        m_offset = m_start_pos - pos;
    } else {
        scene()->clearSelection();
        scene()->setText("");
    }
    m_moved = false;
    m_selected = primitive;
    return true;
}

TEST(TestMeiDocument, ElementsByName) {
    Mei *mei = new Mei();
    Music *mus = new Music();
    Body *body = new Body();
    Staff *staff = new Staff();
    Staff *s2 = new Staff();
    Note *n1 = new Note();
    string wantedId = n1->getId();
    Note *n2 = new Note();
    Note *n3 = new Note();
    Note *n4 = new Note();
    
    mei->addChild(mus);
    mus->addChild(body);
    body->addChild(staff);
    body->addChild(s2);
    staff->addChild(n1);
    staff->addChild(n2);
    staff->addChild(n3);
    s2->addChild(n4);
    
    MeiDocument *doc = new MeiDocument();
    doc->setRootElement(mei);
    
    std::vector<MeiElement*> notes = doc->getElementsByName("note");
    ASSERT_EQ(4, notes.size());
    
    std::vector<MeiElement*> rests = doc->getElementsByName("rest");
    ASSERT_EQ(0, rests.size());

    // After adding the root element, making a new element works
    Note *n5 = new Note();
    staff->addChild(n5);
    
    std::vector<MeiElement*> notes_new = doc->getElementsByName("note");
    ASSERT_EQ(5, notes_new.size());
    
}

static int test_torus()
{
  printf("making torus\n");
  Body* torus = GeometryModifyTool::instance()->torus(1, .2);
  if(!torus)
  {
    printf("failed to make torus\n");
    return 1;
  }
  CubitBox comp_box(CubitVector(-1.2,-1.2,-0.2), CubitVector(1.2,1.2,0.2));
  CubitBox bnd_box = torus->bounding_box();

  bool identical =  cubit_box_identical(bnd_box, comp_box, GEOMETRY_RESABS*2.0, true);
  if (identical)
    return 0;

  if( bnd_box < comp_box || bnd_box > comp_box*1.09)
  {
    printf("boxes not identical\n");
    return 1;
  }
  return 0;
}

/**
* Convenience constructor.
*/
BodyActuator::BodyActuator(const Body& body, 
                           const SimTK::Vec3& point,
                           bool pointIsGlobal,
                           bool spatialForceIsGlobal)
{
    setAuthors("Soha Pouya, Michael Sherman");
    constructInfrastructure();

    updConnector<Body>("body").set_connected_to_name(body.getName());

    set_point(point); // origin
    set_point_is_global(pointIsGlobal);
    set_spatial_force_is_global(spatialForceIsGlobal);
}

	int w_Body_applyImpulse(lua_State * L)
	{
		Body * t = luax_checkbody(L, 1);
		float jx = (float)luaL_checknumber(L, 2);
		float jy = (float)luaL_checknumber(L, 3);

		if(lua_gettop(L) == 3)
		{
			t->applyImpulse(jx, jy);
		}
		else if(lua_gettop(L) == 5)
		{
			float rx = (float)luaL_checknumber(L, 4);
			float ry = (float)luaL_checknumber(L, 5);
			t->applyImpulse(jx, jy, rx, ry);
		}
		else
		{
			return luaL_error(L, "Wrong number of parameters.");
		}

		return 0;
	}

DrawList::Item::Item(const Body &body, Point pos, Point blur, float cloak, float clip, int swizzle, int step)
	: position{static_cast<float>(pos.X()), static_cast<float>(pos.Y())}, clip(clip), flags(swizzle)
{
	Body::Frame frame = body.GetFrame(step);
	tex0 = frame.first;
	tex1 = frame.second;
	flags |= static_cast<uint32_t>(frame.fade * 256.f) << 8;
	
	double width = body.Width();
	double height = body.Height();
	Point unit = body.Facing().Unit();
	Point uw = unit * width;
	Point uh = unit * height;
	
	if(clip < 1.)
	{
		// "clip" is the fraction of its height that we're clipping the sprite
		// to. We still want it to start at the same spot, though.
		pos -= uh * ((1. - clip) * .5);
		position[0] = static_cast<float>(pos.X());
		position[1] = static_cast<float>(pos.Y());
		uh *= clip;
	}
	
	// (0, -1) means a zero-degree rotation (since negative Y is up).
	transform[0] = -uw.Y();
	transform[1] = uw.X();
	transform[2] = -uh.X();
	transform[3] = -uh.Y();
	
	// Calculate the blur vector, in texture coordinates.
	this->blur[0] = unit.Cross(blur) / (width * 4.);
	this->blur[1] = -unit.Dot(blur) / (height * 4.);

	if(cloak > 0.)
		Cloak(cloak);
}

    void onItemAdded(Item* item)
        {
            MessageView* mv = MessageView::instance();

            if(BodyItem* bodyItem = dynamic_cast<BodyItem*>(item)){
                Body* body = bodyItem->body();
                for(size_t i=0; i < body->numDevices(); ++i){
                    Device* device = body->device(i);
                    if(!camera){
                        camera = dynamic_pointer_cast<Camera>(device);
                        if(camera){
                            mv->putln(format("RTCVisionSensorSamplePlugin: Detected Camera \"%1%\" of %2%.")
                                      % camera->name() % body->name());
                            visionSensorSampleRTC->setCameraLocalT(camera->T_local());
                        }
                    }
                    if(!rangeSensor){
                        rangeSensor = dynamic_pointer_cast<RangeSensor>(device);
                        if(rangeSensor){
                            mv->putln(format("RTCVisionSensorSamplePlugin: Detected RangeSensor \"%1%\" of %2%.")
                                      % rangeSensor->name() % body->name());
                            visionSensorSampleRTC->setRangeSensorLocalT(rangeSensor->T_local());
                        }
                    }
                }
            }else if(PointSetItem* pointSetItem = dynamic_cast<PointSetItem*>(item)){
                if(pointSetItem->name() == "RangeCameraPoints"){
                    pointSetFromRangeCamera = pointSetItem->pointSet();
                    mv->putln("RTCVisionSensorSamplePlugin: Detected PointSetItem \"RangeCameraPoints\"");
                    visionSensorSampleRTC->setPointSetFromRangeCamera(pointSetFromRangeCamera);
                } else if(pointSetItem->name() == "RangeSensorPoints"){
                    pointSetFromRangeSensor = pointSetItem->pointSet();
                    mv->putln("RTCVisionSensorSamplePlugin: Detected PointSetItem \"RangeSensorPoints\"");
                    visionSensorSampleRTC->setPointSetFromRangeSensor(pointSetFromRangeSensor);
                }
            }
        }

void BodyItemImpl::doAssign(Item* srcItem)
{
    BodyItem* srcBodyItem = dynamic_cast<BodyItem*>(srcItem);
    if(srcBodyItem){
        // copy the base link property
        Link* baseLink = 0;
        Link* srcBaseLink = srcBodyItem->currentBaseLink();
        if(srcBaseLink){
            baseLink = body->link(srcBaseLink->name());
            if(baseLink){
                setCurrentBaseLink(baseLink);
            }
        }
        // copy the current kinematic state
        Body* srcBody = srcBodyItem->body();
        for(int i=0; i < srcBody->numLinks(); ++i){
            Link* srcLink = srcBody->link(i);
            Link* link = body->link(srcLink->name());
            if(link){
                link->q() = srcLink->q();
            }
        }

        if(baseLink){
            baseLink->p() = srcBaseLink->p();
            baseLink->R() = srcBaseLink->R();
        } else {
            body->rootLink()->p() = srcBody->rootLink()->p();
            body->rootLink()->R() = srcBody->rootLink()->R();
        }
        zmp = srcBodyItem->impl->zmp;

        initialState = srcBodyItem->impl->initialState;
        
        self->notifyKinematicStateChange(true);
    }
}

static int l_get_gps(lua_State *l)
{
	Player *player = LuaObject<Player>::CheckFromLua(1);
	vector3d pos = Pi::player->GetPosition();
	double center_dist = pos.Length();
	auto frame = player->GetFrame();
	if(frame) {
		Body *astro = frame->GetBody();
		if(astro && astro->IsType(Object::TERRAINBODY)) {
			TerrainBody* terrain = static_cast<TerrainBody*>(astro);
			if (!frame->IsRotFrame())
				frame = frame->GetRotFrame();
			vector3d surface_pos = pos.Normalized();
			double radius = 0.0;
			if (center_dist <= 3.0 * terrain->GetMaxFeatureRadius()) {
				radius = terrain->GetTerrainHeight(surface_pos);
			}
			double altitude = center_dist - radius;
			vector3d velocity = player->GetVelocity();
			double vspeed = velocity.Dot(surface_pos);
			if (fabs(vspeed) < 0.05) vspeed = 0.0; // Avoid alternating between positive/negative zero

			//			RefreshHeadingPitch();

			if (altitude < 0) altitude = 0;
			LuaPush(l, altitude);
			LuaPush(l, vspeed);
			const float lat = RAD2DEG(asin(surface_pos.y));
			const float lon = RAD2DEG(atan2(surface_pos.x, surface_pos.z));
			LuaPush(l, lat);
			LuaPush(l, lon);
			return 4;
			//				}
		}
	}
	return 0;
}

Body* System::processChild(pugi::xml_node tmpRoot, Body* parent)
{
    //Deal with the root node
    xml_node tmpNode;
    std::string tmpName = tmpRoot.child_value("name");
    std::string tmpType = tmpRoot.child_value("type");
    std::string tmpDesc = tmpRoot.child_value("desc");
    int tmpRadius = atoi(tmpRoot.child_value("radius"));
    std::string tmpTexture = tmpRoot.child_value("texture");
    int tmpOrbitRadius = atoi(tmpRoot.child_value("orbit_radius"));
    f32 tmpOrbitSpeed = atof(tmpRoot.child_value("orbit_speed"));
    int tmpBrightness = atoi(tmpRoot.child_value("bright"));

    
    core::vector3df tmpDefaultPos = parent->getPosition();
    tmpDefaultPos.X += tmpOrbitRadius;
    
    //core::vector3df(tmpOrbitRadius,0.0f,0.0f);

    Body* rootBody = new Body(this->smgr,this->driver,tmpDefaultPos,tmpRadius,tmpName,tmpDesc,tmpType,tmpOrbitRadius,tmpOrbitSpeed,parent,tmpTexture,tmpBrightness);
    tmpNode = tmpRoot.child("children").first_child();
    bool morePlanets = true;

    while(tmpNode && morePlanets){
        rootBody->addChild(processChild(tmpNode,rootBody));
        if(tmpNode.next_sibling()){
            tmpNode = tmpNode.next_sibling();
            continue;
        }
        else{
            morePlanets=false;
        }
    }

    return rootBody;
}

// Checks ship path to destination for obstacles.
// clear_angle is angle
// returns true if path is clear, false if there is an obstacle.
static bool CheckClearPath(Ship* ship, Frame* destination_frame, vector3d destination_pos, float clear_angle = 0.0f)
{
	Body* sbody = ship->GetFrame()->GetBody();
	if (sbody == nullptr) {
		return true;
	}
	if (sbody->IsType(Object::PLANET) 
		|| (sbody->IsType(Object::SPACESTATION) && ship->GetFrame() != destination_frame)) 
	{
		vector3d ship_pos = ship->GetPositionRelTo(destination_frame);
		vector3d body_pos = sbody->GetPositionRelTo(destination_frame);
		double radius = GetTransitRadius(sbody);
		if (sbody->IsType(Object::PLANET)) {
			radius += 5000.0;
		}
		double ship_to_body_distance;
		vector3d ship_to_body_dir = (body_pos - ship_pos).Normalized(ship_to_body_distance);
		double ship_to_destination_distance;
		vector3d ship_to_destination_dir = (destination_pos - ship_pos).Normalized(ship_to_destination_distance);
		double cos = ship_to_destination_dir.Dot(ship_to_body_dir);
		if (cos > clear_angle) { // Path might be unclear
			double ship_to_closest_distance = cos * ship_to_body_distance;
			vector3d closest_point = ship_pos + (ship_to_destination_dir * ship_to_closest_distance);
			double closest_distance = (closest_point - body_pos).Length();
			if (closest_distance < radius && ship_to_closest_distance < ship_to_destination_distance) {
				return false;
			} else {
				return true;
			}
		} else {
			return true;
		}
	} else {
		return true;
	}
}

void OpenHRPOnlineViewerItemImpl::updateLog
(BodyItemInfo* info, const LinkPositionSequence& links, int numLinks, double time)
{
    BodyMotionItem* motionItem = info->logItem.get();
    if(!motionItem){
        motionItem = info->bodyItem->findChildItem<BodyMotionItem>(info->logName);
        if(!motionItem){
            motionItem = new BodyMotionItem();
            motionItem->setTemporal();
            motionItem->setName(info->logName);
            info->bodyItem->addChildItem(motionItem);
        }
        resetLogItem(info, motionItem);
    }

    if(info->needToSelectLogItem){
        ItemTreeView::instance()->selectItem(motionItem, true);
        info->needToSelectLogItem = false;
    }

    MultiSE3Seq& seq = *motionItem->motion()->linkPosSeq();
    int frame = seq.frameOfTime(time);
    int lastFrame = std::max(0, std::min(frame, seq.numFrames()));
    seq.setNumFrames(frame + 1);

    Body* body = info->bodyItem->body();
    for(int i=lastFrame; i <= frame; ++i){
        MultiSE3Seq::Frame positions = seq.frame(i);
        for(int j=0; j < numLinks; ++j){
            SE3& se3 = positions[j];
            se3.translation() = Eigen::Map<Vector3>(const_cast<double*>(links[j].p));
            Matrix3 Rs = body->link(j)->Rs().transpose();
            se3.rotation() = Eigen::Map<Matrix3>(const_cast<double*>(links[j].R)).transpose() * Rs;
        }
    }
}