C++ vector3d类代码示例

kinematicConstraint::kinematicConstraint(const std::string& _name, int id, tjoint jType, vector3d location, 
										 int ip, vector3d fi, vector3d gi,
										 int jp, vector3d fj, vector3d gj)
	: i(0)
	, j(0)
	, lm(NULL)
	, reactionForce(NULL)
	, sRow(0)
	, iCol(0)
	, jCol(0)
	, nConst(0)
	, max_nnz(0)
	, principal_axis(0)
{
	type = jType;
	name = _name;
	//iName = ip;
	//jName = jp;
	i = ip;
	j = jp;
	ID = id;
	axis = 0.0;

	sp_i = 0.0;
	sp_j = 0.0;
	f_i = fi;
	f_j = fj;
	g_i = gi;
	g_j = gj;
	h_i = fi.cross(gi);
	h_j = fj.cross(gj);
	loc = location;

	//setCoordinates();
}

// returns direction in ship's frame from this ship to target lead position
vector3d Ship::AIGetLeadDir(const Body *target, const vector3d& targaccel, int gunindex)
{
	assert(target);
	if (m_equipment.Get(Equip::SLOT_LASER) == Equip::NONE)
		return target->GetPositionRelTo(this).Normalized();

	const vector3d targpos = target->GetPositionRelTo(this);
	const vector3d targvel = target->GetVelocityRelTo(this);
	// todo: should adjust targpos for gunmount offset

	const int laser = Equip::types[m_equipment.Get(Equip::SLOT_LASER, gunindex)].tableIndex;
	const double projspeed = Equip::lasers[laser].speed;

	vector3d leadpos;
	// avoid a divide-by-zero floating point exception (very nearly zero is ok)
	if( !is_zero_exact(projspeed) ) {
		// first attempt
		double projtime = targpos.Length() / projspeed;
		leadpos = targpos + targvel*projtime + 0.5*targaccel*projtime*projtime;

		// second pass
		projtime = leadpos.Length() / projspeed;
		leadpos = targpos + targvel*projtime + 0.5*targaccel*projtime*projtime;
	} else {
		// default
		leadpos = targpos;
	}

	return leadpos.Normalized();
}

// returns direction in ship's frame from this ship to target lead position
vector3d Ship::AIGetLeadDir(const Body *target, const vector3d& targaccel, int gunindex)
{
	assert(target);
	if (ScopedTable(m_equipSet).CallMethod<int>("OccupiedSpace", "laser_front") == 0)
		return target->GetPositionRelTo(this).Normalized();

	const vector3d targpos = target->GetPositionRelTo(this);
	const vector3d targvel = target->GetVelocityRelTo(this);
	// todo: should adjust targpos for gunmount offset

	double projspeed = 0;
	Properties().Get(gunindex?"laser_rear_speed":"laser_front_speed", projspeed);

	vector3d leadpos;
	// avoid a divide-by-zero floating point exception (very nearly zero is ok)
	if( !is_zero_exact(projspeed) ) {
		// first attempt
		double projtime = targpos.Length() / projspeed;
		leadpos = targpos + targvel*projtime + 0.5*targaccel*projtime*projtime;

		// second pass
		projtime = leadpos.Length() / projspeed;
		leadpos = targpos + targvel*projtime + 0.5*targaccel*projtime*projtime;
	} else {
		// default
		leadpos = targpos;
	}

	return leadpos.Normalized();
}

//member function to initialize a state with nondimensional values
void primVars::NondimensionalInitialize(const idealGas &eos, const vector3d<double> &vel){
  data[0] = 1.0;
  data[4] = 1.0/eos.Gamma();
  data[1] = vel.X() / eos.GetSoS(data[4], data[0]);
  data[2] = vel.Y() / eos.GetSoS(data[4], data[0]);
  data[3] = vel.Z() / eos.GetSoS(data[4], data[0]);
}

vector3d modeler::rotate_axis(vector3d &size, float degrees, int axis) {
	float angle = PI * degrees / 180.0;
	float cos_angle = cos(angle);
	float sin_angle = sin(angle);
	float posX = size.getX();
	float posY = size.getY();
	float posZ = size.getZ();
	float x, y, z;
	switch (axis) {
	case 0: 
		x = cos_angle * posX - sin_angle * posY;
		y = sin_angle * posX + cos_angle * posY;
		z = posZ;
		break;
	case 1: 
		x = cos_angle * posX + sin_angle * posZ;
		y = posY;
		z = -sin_angle * posX + cos_angle * posZ;
		break;
	case 2: 
		x = posX;
		y = cos_angle * posY - sin_angle * posZ;
		z = sin_angle * posY + cos_angle * posZ;
		break;
	default:
		break;
	}
	return vector3d(x, y, z);
}

	void UpdateVBOs() {
		PROFILE_SCOPED()
		//create buffer and upload data
		Graphics::VertexBufferDesc vbd;
		vbd.attrib[0].semantic = Graphics::ATTRIB_POSITION;
		vbd.attrib[0].format   = Graphics::ATTRIB_FORMAT_FLOAT3;
		vbd.attrib[1].semantic = Graphics::ATTRIB_NORMAL;
		vbd.attrib[1].format   = Graphics::ATTRIB_FORMAT_FLOAT3;
		vbd.numVertices = ctx->NUMVERTICES();
		vbd.usage = Graphics::BUFFER_USAGE_STATIC;
		m_vertexBuffer.reset(Pi::renderer->CreateVertexBuffer(vbd));

		GasPatchContext::VBOVertex* vtxPtr = m_vertexBuffer->Map<GasPatchContext::VBOVertex>(Graphics::BUFFER_MAP_WRITE);
		assert(m_vertexBuffer->GetDesc().stride == sizeof(GasPatchContext::VBOVertex));
		
		const Sint32 edgeLen = ctx->edgeLen;
		const double frac = ctx->frac;
		for (Sint32 y=0; y<edgeLen; y++) {
			for (Sint32 x=0; x<edgeLen; x++) {
				const vector3d p = GetSpherePoint(x*frac, y*frac);
				const vector3d pSubCentroid = p - clipCentroid;
				clipRadius = std::max(clipRadius, p.Length());
				vtxPtr->pos = vector3f(pSubCentroid);
				vtxPtr->norm = vector3f(p);

				++vtxPtr; // next vertex
			}
		}
		m_vertexBuffer->Unmap();
	}

vector3d TerrainColorFractal<TerrainColorAsteroid>::GetColor(const vector3d &p, double height, const vector3d &norm) const
{
	double n = m_invMaxHeight*height/2;

	if (n <= 0.02) {
		const double flatness = pow(p.Dot(norm), 6.0);
		const vector3d color_cliffs = m_rockColor[1];

		double equatorial_desert = (2.0)*(-1.0+2.0*octavenoise(12, 0.5, 2.0, (n*2.0)*p)) *
			1.0*(2.0)*(1.0-p.y*p.y);

		vector3d col;
		col = interpolate_color(equatorial_desert, m_rockColor[0], m_greyrockColor[3]);
		col = interpolate_color(n, col, vector3d(1.5,1.35,1.3));
		col = interpolate_color(flatness, color_cliffs, col);
		return col;
	} else {
		const double flatness = pow(p.Dot(norm), 6.0);
		const vector3d color_cliffs = m_greyrockColor[1];

		double equatorial_desert = (2.0)*(-1.0+2.0*octavenoise(12, 0.5, 2.0, (n*2.0)*p)) *
			1.0*(2.0)*(1.0-p.y*p.y);

		vector3d col;
		col = interpolate_color(equatorial_desert, m_greyrockColor[0], m_greyrockColor[2]);
		col = interpolate_color(n, col, m_rockColor[3]);
		col = interpolate_color(flatness, color_cliffs, col);
		return col;
	}
}

vector3f toFloat(vector3d & pts) {
	vector3f opts(pts.size());
	for (int i=0; i<pts.size();i++) {
		Vector3f vec((float) pts[i].x(), (float) pts[i].y(), (float) pts[i].z());
		opts.push_back(vec);
	}
	return opts;
}

static int GetFlipMode(Ship *ship, const vector3d &relpos, const vector3d &relvel)
{
	double dist = relpos.Length();
	double vel = relvel.Dot(relpos) / dist;
	if (vel > 0.0 && vel*vel > 1.7 * ship->GetAccelRev() * dist) return 2;
	if (dist > 100000000.0) return 1;	// arbitrary
	return 0;
}

vector3d TransferPlanner::GetOffsetVel() const {
	if(m_position.ExactlyEqual(vector3d(0., 0., 0.)))
		return vector3d(0., 0., 0.);

	const vector3d pNormal = m_position.Cross(m_velocity);

	return m_dvPrograde * m_velocity.Normalized() +
		   m_dvNormal   * pNormal.Normalized() +
		   m_dvRadial   * m_position.Normalized();
}

static void DrawAtmosphereSurface(Graphics::Renderer *renderer,
	const matrix4x4d &modelView, const vector3d &campos, float rad, Graphics::Material *mat)
{
	const int LAT_SEGS = 20;
	const int LONG_SEGS = 20;
	vector3d yaxis = campos.Normalized();
	vector3d zaxis = vector3d(1.0,0.0,0.0).Cross(yaxis).Normalized();
	vector3d xaxis = yaxis.Cross(zaxis);
	const matrix4x4d invrot = matrix4x4d::MakeRotMatrix(xaxis, yaxis, zaxis).InverseOf();

	renderer->SetTransform(modelView * matrix4x4d::ScaleMatrix(rad, rad, rad) * invrot);

	// what is this? Well, angle to the horizon is:
	// acos(planetRadius/viewerDistFromSphereCentre)
	// and angle from this tangent on to atmosphere is:
	// acos(planetRadius/atmosphereRadius) ie acos(1.0/1.01244blah)
	double endAng = acos(1.0/campos.Length())+acos(1.0/rad);
	double latDiff = endAng / double(LAT_SEGS);

	double rot = 0.0;
	float sinCosTable[LONG_SEGS+1][2];
	for (int i=0; i<=LONG_SEGS; i++, rot += 2.0*M_PI/double(LONG_SEGS)) {
		sinCosTable[i][0] = float(sin(rot));
		sinCosTable[i][1] = float(cos(rot));
	}

	/* Tri-fan above viewer */
	Graphics::VertexArray va(Graphics::ATTRIB_POSITION);
	va.Add(vector3f(0.f, 1.f, 0.f));
	for (int i=0; i<=LONG_SEGS; i++) {
		va.Add(vector3f(
			sin(latDiff)*sinCosTable[i][0],
			cos(latDiff),
			-sin(latDiff)*sinCosTable[i][1]));
	}
	renderer->DrawTriangles(&va, mat, Graphics::TRIANGLE_FAN);

	/* and wound latitudinal strips */
	double lat = latDiff;
	for (int j=1; j<LAT_SEGS; j++, lat += latDiff) {
		Graphics::VertexArray v(Graphics::ATTRIB_POSITION);
		float cosLat = cos(lat);
		float sinLat = sin(lat);
		float cosLat2 = cos(lat+latDiff);
		float sinLat2 = sin(lat+latDiff);
		for (int i=0; i<=LONG_SEGS; i++) {
			v.Add(vector3f(sinLat*sinCosTable[i][0], cosLat, -sinLat*sinCosTable[i][1]));
			v.Add(vector3f(sinLat2*sinCosTable[i][0], cosLat2, -sinLat2*sinCosTable[i][1]));
		}
		renderer->DrawTriangles(&v, mat, Graphics::TRIANGLE_STRIP);
	}
}

static int l_vector_unit(lua_State *L)
{
	LUA_DEBUG_START(L);
	if (lua_isnumber(L, 1)) {
		double x = luaL_checknumber(L, 1);
		double y = luaL_checknumber(L, 2);
		double z = luaL_checknumber(L, 3);
		const vector3d v = vector3d(x, y, z);
		LuaVector::PushToLua(L, v.NormalizedSafe());
	} else {
		const vector3d *v = LuaVector::CheckFromLua(L, 1);
		LuaVector::PushToLua(L, v->NormalizedSafe());
	}
	LUA_DEBUG_END(L, 1);
	return 1;
}

vector3d TerrainColorFractal<TerrainColorVolcanic>::GetColor(const vector3d &p, double height, const vector3d &norm)
{
	double n = m_invMaxHeight*height;
	const double flatness = pow(p.Dot(norm), 6.0);
	const vector3d color_cliffs = m_rockColor[2];		
	double equatorial_desert = (-1.0+2.0*octavenoise(12, 0.5, 2.0, (n*2.0)*p)) *
			1.0*(1.0-p.y*p.y);

	vector3d col;

	if (n > 0.4){
	col = interpolate_color(equatorial_desert, vector3d(.3,.2,0), vector3d(.3, .1, .0));
	col = interpolate_color(n, col, vector3d(.1, .0, .0));
	col = interpolate_color(flatness, color_cliffs, col);
	} else if (n > 0.2){
	col = interpolate_color(equatorial_desert, vector3d(1.2,1,0), vector3d(.9, .3, .0));
	col = interpolate_color(n, col, vector3d(-1.1, -1, .0));
	col = interpolate_color(flatness, color_cliffs, col);
	} else if (n > 0.1){
	col = interpolate_color(equatorial_desert, vector3d(.2,.1,0), vector3d(.1, .05, .0));
	col = interpolate_color(n, col, vector3d(2.5, 2, .0));
	col = interpolate_color(flatness, color_cliffs, col);
	} else {
	col = interpolate_color(equatorial_desert, vector3d(.75,.6,0), vector3d(.75, .2, .0));
	col = interpolate_color(n, col, vector3d(-2, -2.2, .0));
	col = interpolate_color(flatness, color_cliffs, col);
	}
	return col;
}

void HyperspaceCloud::Render(const vector3d &viewCoords, const matrix4x4d &viewTransform)
{
	Render::State::UseProgram(Render::simpleShader);
	glDisable(GL_LIGHTING);
	glEnable(GL_BLEND);
	glPushMatrix();
	glTranslatef(float(viewCoords.x), float(viewCoords.y), float(viewCoords.z));
	
	// face the camera dammit
	vector3d zaxis = viewCoords.NormalizedSafe();
	vector3d xaxis = vector3d(0,1,0).Cross(zaxis).Normalized();
	vector3d yaxis = zaxis.Cross(xaxis);
	matrix4x4d rot = matrix4x4d::MakeRotMatrix(xaxis, yaxis, zaxis).InverseOf();
	glMultMatrixd(&rot[0]);
	// precise to the rendered frame (better than PHYSICS_HZ granularity)
	double preciseTime = Pi::game->GetTime() + Pi::GetGameTickAlpha()*Pi::game->GetTimeStep();

	float radius = 1000.0f + 200.0f*float(noise(10.0*preciseTime, 0, 0));
	if (m_isArrival) {
		make_circle_thing(radius, Color(1.0,1.0,1.0,1.0), Color(0.0,0.0,1.0,0.0));
	} else {
		make_circle_thing(radius, Color(1.0,1.0,1.0,1.0), Color(1.0,0.0,0.0,0.0));
	}
	glPopMatrix();
	glDisable(GL_BLEND);
	glEnable(GL_LIGHTING);
}

double Ship::AIFaceOrient(const vector3d &dir, const vector3d &updir)
{
	double timeStep = Pi::GetTimeStep();
	matrix4x4d rot; GetRotMatrix(rot);
	double maxAccel = GetShipType().angThrust / GetAngularInertia();		// should probably be in stats anyway
	double frameAccel = maxAccel * timeStep;
	
	if (dir.Dot(vector3d(rot[8], rot[9], rot[10])) > -0.999999)
		{ AIFaceDirection(dir); return false; }
	
	vector3d uphead = (updir * rot).Normalized();		// create desired object-space updir
	vector3d dav(0.0, 0.0, 0.0);			// desired angular velocity
	double ang = 0.0;
	if (uphead.y < 0.999999)
	{
		ang = acos(Clamp(uphead.y, -1.0, 1.0));		// scalar angle from head to curhead
		double iangvel = sqrt(2.0 * maxAccel * ang);	// ideal angvel at current time

		double frameEndAV = iangvel - frameAccel;
		if (frameEndAV <= 0.0) iangvel = ang / timeStep;	// last frame discrete correction
		else iangvel = (iangvel + frameEndAV) * 0.5;		// discrete overshoot correction
		dav.z = -iangvel;
	}
	vector3d cav = (GetAngVelocity() - GetFrame()->GetAngVelocity()) * rot;				// current obj-rel angvel
//	vector3d cav = GetAngVelocity() * rot;				// current obj-rel angvel
	vector3d diff = (dav - cav) / frameAccel;			// find diff between current & desired angvel

	SetAngThrusterState(diff);
	return ang;
//	if (diff.x*diff.x > 1.0 || diff.y*diff.y > 1.0 || diff.z*diff.z > 1.0) return false;
//	else return true;
}