C++ MathVector类代码示例

/*
***************************************************************
*
* Draws the face normals on the weapon model
*
***************************************************************
*/
void GLWidget::drawWeaponFaceNormals()
{
    glDisable(GL_LIGHTING);
    glColor3f(0.0, 0.0, 1.0);
    for(int i = 0; i < weaponReader_.numberOfTriangles(); i++)
    {
        int indexOne = 0;
        int indexTwo = 0;
        int indexThree = 0;
        weaponReader_.retrieveTriangleVertexIndicies(i, &indexOne,
                &indexTwo, &indexThree);

        MathVector* faceNormals = weaponReader_.faceNormals()->at(i);


        VertexCoordinate vertexOne = weaponReader_.retrieveVertexCoordinatesAt(indexOne);
        VertexCoordinate vertexTwo = weaponReader_.retrieveVertexCoordinatesAt(indexTwo);
        VertexCoordinate vertexThree = weaponReader_.retrieveVertexCoordinatesAt(indexThree);

        VertexCoordinate middleOfTriangle;
        middleOfTriangle.x = ((vertexOne.x + vertexTwo.x + vertexThree.x)/3);
        middleOfTriangle.y = ((vertexOne.y + vertexTwo.y + vertexThree.y)/3);
        middleOfTriangle.z = ((vertexOne.z + vertexTwo.z + vertexThree.z)/3);

        glBegin(GL_LINES);
        glVertex3f(middleOfTriangle.x, middleOfTriangle.y,
                   middleOfTriangle.z);
        glVertex3f((faceNormals->x()*2)+middleOfTriangle.x, (faceNormals->y()*2)+middleOfTriangle.y,
                   (faceNormals->z()*2)+middleOfTriangle.z);
        glEnd();
    }
    glEnable(GL_LIGHTING);
}

MathVector tripleProduct(MathVector a, MathVector b, MathVector c)
{
	MathVector U = b.multiply(c.dotProduct(a));
	MathVector V = a.multiply(c.dotProduct(b));

	return U.subtractVectors(V);
}

void CBaseBrushDraw::UpdateControlPoints()
{
	GetBrushGridIndexs(m_vecBrushGridIndices, m_vecBrushVertexs);
	if ( m_lastBrushVertexs == m_vecBrushVertexs )
		return;

	m_lastBrushVertexs = m_vecBrushVertexs;

	int nWidth = m_nOuterWidth  * 2;
	MathVector<CVector3f> temp;
	CVector3f leftbottom, rightbottom, righttop, lefttop;
	leftbottom  = m_vecBrushVertexs[0];
	rightbottom = m_vecBrushVertexs[0+nWidth];
	righttop    = m_vecBrushVertexs[m_vecBrushVertexs.size()-1];
	lefttop     = m_vecBrushVertexs[m_vecBrushVertexs.size()-1-nWidth];

	leftbottom.y += 1.0f;
	rightbottom.y += 1.0f;
	righttop.y += 1.0f;
	leftbottom.y += 1.0f;

	temp.push_back(leftbottom);
	temp.push_back(rightbottom);
	temp.push_back(righttop);
	temp.push_back(lefttop);

	m_curveSampler.SetControlPoint(temp, true);
}

MathVector Math::normal(MathVector a, MathVector b, MathVector c)
{
MathVector r1;
MathVector r2;
MathVector output;
	r2=c-b;
	r1=b-a;
	output = r1.cross(r2);
	output=(1.0f/output.size())*output;
	return output;
}

void change_route( MathVector mSource, MathVector mDestination )
{
    printf( "Now Showing Route : \n" );
    mSource.print();
    mDestination.print();
    map2D.map_route         ( multiRoute, mSource, mDestination );
    //robot.m_route.create_from_multi( multiRoute );
    //robot.plan_steps        ( 2.0*12 );
    
    robot.gl_register       ( );
    robot.m_glide_index = 0;
}

	const MathVector<T, dimension> reflected(const MathVector<T, dimension>& other) const {
		MathVector<T, dimension> output;

		output = (*this) - other * T(2.0) * other.dot(*this);

		return output;
	}

void Particle::updateVelocity()
{
	if (parameters.fips)
	{
		MathVector influence;
		influence.fillValues(v.size(), 0.0);
		for(auto n: neighbours)
			influence = influence + randDouble(0.0, parameters.c) * (n->p - x);
		influence = (1.0 / neighbours.size()) * influence;
		v = parameters.w * v + influence;
	}
	else
	{
		v = parameters.w * v + randDouble(0.0, parameters.c) * (p - x) + randDouble(0.0, parameters.c) * (best->p - x);
	}
}

bool containsOrigin(std::vector<MathVector> &simplex, MathVector &direction)
{
	MathVector a = simplex.back();
	MathVector b, c, ab, ac, abPerp, acPerp;
	MathVector ao = a.negate();

	if(simplex.size() == 3)
	{
		b = simplex[0];
		c = simplex[1];

		ab = b.subtractVectors(a);
		ac = c.subtractVectors(a);

		abPerp = tripleProduct(ac, ab, ab);
		acPerp = tripleProduct(ab, ac, ac);

		if(abPerp.dotProduct(ao) > 0)
		{
			simplex.erase(simplex.begin() + 1);
			direction = abPerp;
		} else if (acPerp.dotProduct(ao) > 0)
		{
			simplex.erase(simplex.begin());
			direction = acPerp;
		} else
		{
			return true;
		}
	} else
	{
		b = simplex[0];
		ab = b.subtractVectors(a);
		abPerp = tripleProduct(ab, ao, ab);

		if(abPerp.getX() == 0 || abPerp.getY() == 0)
		{
			direction = ab.perpendicular();
		} else
		{
			direction = abPerp;
		}
	}
	return false;
}

minkowskiDifference_t buildMinkowskiDifference(std::vector<MathVector> a, std::vector<MathVector> b)
{
	MathVector direction = MathVector(1,1);
	std::vector<MathVector> simplex;
	simplex.push_back(getSupportVertex(a, b, direction));
	minkowskiDifference_t difference;

	direction = direction.negate();

	while(true)
	{
		simplex.push_back(getSupportVertex(a, b, direction));
		if(simplex.back().dotProduct(direction) <= 0)
		{
			difference.colliding = false;
			difference.collisionNormal = MathVector(0,0);
			difference.collisionDepth = 0;
			return difference;
		} else if(containsOrigin(simplex, direction) && simplex.size() == 3)
		{
			while(true)
			{
				//Perform EPA to get collision normal and penetration distance
				Edge_t e = findClosestEdge(simplex);

				MathVector p = getSupportVertex(a, b, direction);
				double d = p.dotProduct(e.normal);
//				std::cout << d - e.distance << std::endl;
//				std::cout << "Simplex size: " << simplex.size() << std::endl;
				if(d - e.distance < TOLERANCE)
				{
					difference.collisionNormal = e.normal;
					difference.collisionDepth = d;
					difference.colliding = true;
					return difference;
				} else
				{
//					std::cout << "Closest edge not found in this iteration, adding point to simplex and continuing." << std::endl;
					simplex.insert((simplex.begin()+e.index),p);
				}
			}
		}
	}
}

void move_sideways( float mAmount )
{
    MathVector forward(3);
    forward[0] = centerX - eyeX;
    forward[1] = centerY - eyeY;
    forward[2] = centerZ - eyeZ;
    MathVector perp = forward.get_perp_xz();
    perp.unitize();
    perp *= mAmount;

    eyeX    += perp[0];
    eyeZ    += perp[2];
    centerX += perp[0];
    centerZ += perp[2];
	theWorld.look_at( eyeX,    eyeY,    eyeZ,
					 centerX, centerY, centerZ,
					 0.0, 1.0, 0.0 );					
    
}

// overloaded multiplication of matrix by a vector 
MathVector MathMatrix::operator*(const MathVector& v) const
{
	if (n != v.size()) { throw "Matrix and Vector do not"; }
	//Create matrix object of the correct size to hold the resulting matrix from multiplication
	MathVector temp(v.size());
	//Go across the rows of the matrix the method was called on
	for (int i=0; i<nrows; i++)
	{
		//Declare empty variable to hold the multiplication result
		double sum = 0;
		//Go across the columns on the matrix that was passed as a parameter
		for (int j=0; j<ncols; j++)
		{
			sum+=(*this)(i, j) * v[0];
		}
		//Set element in temp at the corresponding loop iteration index to the result
		temp[i] = sum;
	}
	return temp;
}

MathVector getFurthestPoint(MathVector direction, std::vector<MathVector> polygon)
	{
		double greatestDotProduct = -std::numeric_limits<double>::max();
		double currentDotProduct;
		MathVector currentVertex;
		MathVector bestVertex;
		for(int i = 0; i < polygon.size(); i++)
		{
			currentVertex = polygon[i];
			currentDotProduct = currentVertex.dotProduct(direction);
			if(currentDotProduct > greatestDotProduct)
			{
				greatestDotProduct = currentDotProduct;
				bestVertex = currentVertex;
			}
		}

		return bestVertex;

	}

Quat Math::rot2Quat( MathVector in)
{
	MathVector inv = in;
	Quat nq;
	double invs;
	double phi;
	MathVector unitV;
	invs = inv.size();
		if(invs != 0)
		{
			unitV = 1/invs * inv ;
		}
		else
		{
			unitV = MathVector(1,0,0);
		}
	phi = invs * 3.14159265/180;
	nq.v = sin(phi/2) * unitV;
	nq.scale = cos(phi/2);
	return nq;
}

void sim_read_robot_angles( long& mRobot_id, MathVector& mNewAngles )
{
    if (ipc_memory_valid_pointer()==false)
        throw IPC_Error;
    
    if (ipc_memory_sim->Command == COMMAND_ROBOT_ANGLES)
    {
        mRobot_id     =   ipc_memory_sim->robot_id;

        // Dimension the new MathVector:
        int dimension =   (ipc_memory_sim->object_datum1 & 0xFF);
        if (dimension> MAX_SERVOS) dimension = MAX_SERVOS;
        mNewAngles.dimension(dimension);

        // EXTRACT the angles:
        for (int i=0; i<dimension; i++)
            mNewAngles[i] = ipc_memory_sim->servo_angles[i];
    }
}

typename MathVector<T>::value_type operator*(const MathVector<T>& lhs, const MathVector<T>& rhs)
{
  return lhs.dotProduct(rhs);
}