Пример #1
0
	Vector3D Vector3D::GenerateRandomVector(Vector3D & i_Min, Vector3D & i_Max)
	{
		return Vector3D(
			RandInRange(i_Min.getX(), i_Max.getX()),
			RandInRange(i_Min.getY(), i_Max.getY()),
			RandInRange(i_Min.getZ(), i_Max.getZ()));
	}
Пример #2
0
/*
 * Compute the vonMises function for the sheerAndStress values
 * The vonMises function allows to determine (locally) if a object is gonna break;
 * in our case this is gonna be used to detemine if a adhesive bond (= local link) is gonna break
 * vonMises: sigma_v = 1/sqrt(2) * sqrt(2*Sigma_y² + 6*Tau_x² + 6*Tau_z²)
 *
 */
dReal StickyObj::vonMisesFunction(Vector3D vec){
	// TO DO : revert the threshold value to have this one squared !! (REMOVE SQRT)
	//vec corresponds to Sigma_y, Tau_x, Tau_z
	dReal sigY = vec.getX();
	if(vec.getX() < 0){
		sigY =0;
	}
	return (1/sqrt(2))*sqrt(2*sigY*sigY + 6*vec.getY()*vec.getY() + 6*vec.getZ()*vec.getZ());
}
Пример #3
0
Length Vector3D::getGroundRangeTo(const Vector3D &other) {
	Vector3D diff = getVectorTo(other);
	double x = diff.getX().getDoubleValue(Length::METERS);
	double y = diff.getY().getDoubleValue(Length::METERS);
	double dist = sqrt( x*x + y*y );
	return Length(dist, Length::METERS);
}
void Margin::draw()
{

	Vector3D * scale = getScale();

	getVSML()->loadIdentity(VSMathLib::MODEL);

	// Margin
	getVSML()->pushMatrix(VSMathLib::MODEL);
	getVSML()->translate(getPosition()->getX(), getPosition()->getY(), getPosition()->getZ());
	getVSML()->scale(scale->getX(), scale->getY(), scale->getZ());

	// cube
	getVSML()->pushMatrix(VSMathLib::MODEL);
	getVSML()->translate(-0.5f, -0.5f, -0.5f);
	glUseProgram((*getShader()).getProgramIndex());
	
	getVSML()->matricesToGL();
	glBindVertexArray(vaoMargin);
	glDrawElements(GL_TRIANGLES, faceCountMargin * 3, GL_UNSIGNED_INT, 0);
	getVSML()->popMatrix(VSMathLib::MODEL);
	// cube

	getVSML()->popMatrix(VSMathLib::MODEL);
	// Margin
}
Пример #5
0
/**
 * Creates a new ODE_UniversalJoint.
 *
 * @param body1 the first body to connect the joint to.
 * @param body2 the second body to connect the joint to.
 * @return the new ODE_UniversalJoint.
 */
dJointID ODE_UniversalJoint::createJoint(dBodyID body1, dBodyID body2) {

	if(mJointAxis1Point1->get().equals(mJointAxis1Point2->get())
		|| mJointAxis2Point1->get().equals(mJointAxis2Point2->get())) {
		Core::log("Invalid axes for ODE_UniversalJoint.");
		return 0;
	}

	//if one of the bodyIDs is null, the joint is connected to a static object.
	dJointID newJoint = dJointCreateUniversal(mWorldID, mGeneralJointGroup);
	dJointAttach(newJoint, body1, body2);
	
	Vector3D anchor = mAnchorPoint->get() ;
	Vector3D axis1;
	Vector3D axis2;
	
	axis1.set(mJointAxis1Point2->getX() - mJointAxis1Point1->getX(), 
			  mJointAxis1Point2->getY() - mJointAxis1Point1->getY(), 
			  mJointAxis1Point2->getZ() - mJointAxis1Point1->getZ());

	axis2.set(mJointAxis2Point2->getX() - mJointAxis2Point1->getX(), 
			  mJointAxis2Point2->getY() - mJointAxis2Point1->getY(), 
			  mJointAxis2Point2->getZ() - mJointAxis2Point1->getZ());
	
	dJointSetUniversalAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ());
	dJointSetUniversalAxis1(newJoint, axis1.getX(), axis1.getY(), axis1.getZ());
	dJointSetUniversalAxis2(newJoint, axis2.getX(), axis2.getY(), axis2.getZ());

	return newJoint; 
}
Пример #6
0
/**
 * Called when a parameter Value changed.
 * @param value the value that changed.
 */
void WavefrontBody::valueChanged(Value *value) {
	SimBody::valueChanged(value);

	if(value == 0) {
		return;
	} 
	else if(mReferenceObject != 0 
		&& (value == mReferenceObject->getPositionValue() 
			|| value == mReferenceObject->getQuaternionOrientationValue()))
	{
		Quaternion localPos(0.0, 
						mLocalPosition->getX(), 
						mLocalPosition->getY(), 
						mLocalPosition->getZ());
		Quaternion bodyOrientationInverse = 
			mReferenceObject->getQuaternionOrientationValue()->get().getInverse();
		Quaternion rotatedLocalPosQuat = mReferenceObject->getQuaternionOrientationValue()->get() 
			* localPos * bodyOrientationInverse;
		Vector3D rotatedLocalPos(rotatedLocalPosQuat.getX(), 
								rotatedLocalPosQuat.getY(), 
								rotatedLocalPosQuat.getZ());
		mPositionValue->set(mReferenceObject->getPositionValue()->get() + rotatedLocalPos);
	}
	else if(mReferenceObject != 0 && value == mReferenceObject->getOrientationValue()) {
		Vector3D orientation = mReferenceObject->getOrientationValue()->get() + mLocalOrientation->get();
		while(orientation.getX() > 180.0) {
			orientation.setX(orientation.getX() - 360.0);
		}
		while(orientation.getX() < -180.0) {
			orientation.setX(orientation.getX() + 360.0);
		}
		while(orientation.getY() > 180.0) {
			orientation.setY(orientation.getY() - 360.0);
		}
		while(orientation.getY() < -180.0) {
			orientation.setY(orientation.getY() + 360.0);
		}
		while(orientation.getZ() > 180.0) {
			orientation.setZ(orientation.getZ() - 360.0);
		}
		while(orientation.getZ() < -180.0) {
			orientation.setZ(orientation.getZ() + 360.0);
		}
		getOrientationValue()->set(orientation);
	}
}
Пример #7
0
NorthBearingAngle Vector3D::getBearingTo(const Vector3D &other) {
	Vector3D diff = getVectorTo(other);
	if (diff == Vector3D::ZERO)
		return NorthBearingAngle::NORTH;
	double x = diff.getX().getDoubleValue(Length::METERS);
	double y = diff.getY().getDoubleValue(Length::METERS);
	Angle ang( atan2(y,x), Angle::RADIANS );
	return ang.toNorthBearing();
}
Пример #8
0
/**
 * Creates a new ODE_HingeJoint.
 *
 * @param body1 the first body to connect the joint to.
 * @param body2 the second body to connect the joint to.
 * @return the new ODE_HingeJoint.
 */
dJointID ODE_HingeJoint::createJoint(dBodyID body1, dBodyID body2) {
	if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get())) {
		Core::log("Invalid axes for ODE_HingeJoint.");
		return 0;
	}
	//if one of the bodyIDs is null, the joint is connected to a static object.
	dJointID newJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
	dJointAttach(newJoint, body1, body2);
	
	Vector3D anchor = mJointAxisPoint1->get() ;
	Vector3D axis = mJointAxisPoint2->get() ;
	
	axis.set(mJointAxisPoint2->getX() - mJointAxisPoint1->getX(), mJointAxisPoint2->getY() - mJointAxisPoint1->getY(), mJointAxisPoint2->getZ() - mJointAxisPoint1->getZ());
	
	dJointSetHingeAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ());
	dJointSetHingeAxis(newJoint, axis.getX(), axis.getY(), axis.getZ());
	return newJoint; 
}
Пример #9
0
	double SimpleLightSource::getBrightness(
			const Vector3D &globalPosition, const bool &restrictToHorizontal)
	{
		double brightness = 0.0;
		double distance = 0.0;

		if(restrictToHorizontal) {
			if(mSwitchYZAxes != 0 && mSwitchYZAxes->get()) {
				distance = Math::abs(Math::distance(
							QPointF(globalPosition.getX(),
								globalPosition.getZ()),
							QPointF(getPositionValue()->getX(),
								getPositionValue()->getZ()
				)));
			}
			else {
				distance = Math::abs(Math::distance(
							QPointF(globalPosition.getX(),
								globalPosition.getY()),
							QPointF(getPositionValue()->getX(),
								getPositionValue()->getY()
				)));
			}
		}
		else {
			distance = Math::distance(globalPosition,
					getPositionValue()->get());
		}
		
		double radius = getRadius();
		
		if(radius == 0.0 || distance > radius) {
			return brightness;
		}
		
		double relDis = radius - distance;

		switch(mDistributionType->get()) {
			case 0: // uniform distribution
				brightness = getCenterBrightness();
				break;
			case 1: // linear decay
				brightness = relDis / radius * getCenterBrightness();
				break;
			case 2: // quadratic decay
				brightness = (relDis * relDis) / (radius * radius)
					* getCenterBrightness();
				break;
			case 3: // cubic decay
				brightness = (relDis * relDis * relDis)
					/ (radius * radius * radius)
					* getCenterBrightness();
				break;				
		}
		return brightness;
	}
Пример #10
0
/**
 * Computes the point intersected and the normal at this point when a ray is cast. 
 * Point Hit and Normal Hit attributes are only modified if intersection is detected, otherwise
 * returns only false result 
 * 
 * To test intersection with a sphere, we locate the point along the ray closest
 * to the center of the sphere and compare it with the radius.
 * 
 * @param ray Ray to test intersection
 * @param pointHit Point hit by the ray, if intersection is detected
 * @param normalHit Normal of the surface at the point that is hit, if intersection is detected
 * @return Boolean indicating whether intersection was detected or not
 */
bool Sphere::hasIntersection(Ray* ray, Point3D* point_hit, Vector3D* normal_hit){
    //Geometric Solution
    
    //1. Generate a Vector going from the origin of the Ray to the center of the Sphere
    double vector_to_sphere_center_x = this->center_->getX() - ray->getOrigin()->getX();
    double vector_to_sphere_center_y = this->center_->getY() - ray->getOrigin()->getY();
    double vector_to_sphere_center_z = this->center_->getZ() - ray->getOrigin()->getZ();
    double distance_to_sphere_center = sqrt(vector_to_sphere_center_x * vector_to_sphere_center_x + 
                                            vector_to_sphere_center_y * vector_to_sphere_center_y + 
                                            vector_to_sphere_center_z * vector_to_sphere_center_z);
    
    //2. Compute the Dot Product of this vector and the original Ray
    //Note: This also the distance from the ray origin to a point that forms a right angle with the sphere center pt
    double distance_to_test_point = vector_to_sphere_center_x * ray->getDirection()->getX() + 
                                    vector_to_sphere_center_y * ray->getDirection()->getY() + 
                                    vector_to_sphere_center_z * ray->getDirection()->getZ();
    
    //3. Reject if the Sphere is not in the direction of the Ray (Dot Product is Negative)
    if(distance_to_test_point < 0){
        return false;
    }
    
    //4. Compute the distance to the closest point from the sphere center that exists along the ray by using the Pythagorean theorem
    //     along with the right triangle formed by the ray, the vector from the ray origin to the center, and the vector from the center to the closest pt on the ray
    double distance_from_sphere_center = sqrt(std::abs(distance_to_sphere_center * distance_to_sphere_center - distance_to_test_point * distance_to_test_point));
    
    //5. If this distance is greater than the radius, reject
    if(distance_from_sphere_center > this->radius_){
        return false;
    }
    
    //Compute the intersection pt and the normal
    
    //1. Find the amount of the ray penetrating the sphere up to the test point using the triangle formed by
    //     the radius and the distance to the closest pt on the sphere
    double penetration_amount = sqrt(this->radius_ * this->radius_ - distance_from_sphere_center * distance_from_sphere_center);
    
    //2. Subtract this distance from the distance from the origin to the closest pt on the ray to determine
    //      the distance from the origin to the intersection pt
    double distance_to_intersection = distance_to_test_point - penetration_amount;
    
    //3. Using parametric coordinates, find the point in 3D space where the sphere was intersected by the ray
    Point3D intersection_point = ray->findPoint(distance_to_intersection);
    
    //4. Return the intersection point and the normal at that point
    point_hit->setX(intersection_point.getX());
    point_hit->setY(intersection_point.getY());
    point_hit->setZ(intersection_point.getZ());
    
    Vector3D normal = getNormalAt(&intersection_point);
    normal_hit->setX(normal.getX());
    normal_hit->setY(normal.getY());
    normal_hit->setZ(normal.getZ());
    
    return true;
}
  static Vector3D<T> subtract(Vector3D<T> v1, Vector3D<T> v2)
  {
    Vector3D<T> result = Vector3D<T>(v1._x, v1._y, v1._z);

    result._x -= v2.getX();
    result._y -= v2.getY();
    result._z -= v2.getZ();

    return result;
  }
  //
  // Static methods
  //
  static Vector3D<T> add(Vector3D<T> v1, Vector3D<T> v2)
  {
    Vector3D<T> result = Vector3D<T>(v1._x, v1._y, v1._z);

    result._x += v2.getX();
    result._y += v2.getY();
    result._z += v2.getZ();

    return result;
  }
Пример #13
0
double Sphere::findIntersection(Ray ray){
	Vector3D rayOrigin = ray.getOrigin();
	double rayOriginX = rayOrigin.getX();
	double rayOriginY = rayOrigin.getY();
	double rayOriginZ = rayOrigin.getZ();

	Vector3D rayDirection = ray.getDirection();
	double rayDirectionX = rayDirection.getX();
	double rayDirectionY = rayDirection.getY();
	double rayDirectionZ = rayDirection.getZ();

	double centerX = this->center.getX();
	double centerY = this->center.getY();
	double centerZ = this->center.getZ();

	double a = 1.0; //Normalized.
	double b = (2*(rayOriginX - centerX)*rayDirectionX) + (2*(rayOriginY - centerY)*rayDirectionY) + (2*(rayOriginZ - centerZ)*rayDirectionZ);
	double c = std::pow(rayOriginX - centerX, 2) + std::pow(rayOriginY - centerY, 2) + std::pow(rayOriginZ - centerZ, 2) - (this->radius*this->radius);

	double discriminant = b*b - 4 *c; //b^2 -4ac

	if (discriminant > 0){
		//The ray intersects the sphere, intersecting two sides.

		//First root
		double root1 = ((-1*b - std::sqrt(discriminant))/2) - 0.00001;
		if (root1 > 0){
			//The first root is the smallest positive root.
			return root1;
		}
		else {
			//The second root is the smallest positive root.
			double root2 = ((std::sqrt(discriminant) - b)/2) - 0.00001;
			return root2;
		}
	}
	else {
		//The ray missed the sphere.
		return -1;
	}
}
  static Vector3D<T> normalize(Vector3D<T> v)
  {
    T length = getVector3DLength(v);

    Vector3D<T> result;

    result.setX(v.getX() / length);
    result.setY(v.getY() / length);
    result.setZ(v.getZ() / length);

    return result;
  }
Пример #15
0
Triangle::Triangle(const Vector3D & epA, const Vector3D & epB,
                   const Vector3D & epC, Color color)
                    : epA(epA), epB(epB), epC(epC), color(color) {
    normal = (epC - epA).crossProduct(epB - epA).normalize();
    distance = normal.dotProduct(epA);
    setBBox(std::min({epA.getX(), epB.getX(), epC.getX()}),
            std::min({epA.getY(), epB.getY(), epC.getY()}),
            std::min({epA.getZ(), epB.getZ(), epC.getZ()}),
            std::max({epA.getX(), epB.getX(), epC.getX()}),
            std::max({epA.getY(), epB.getY(), epC.getY()}),
            std::max({epA.getZ(), epB.getZ(), epC.getZ()}));
}
Пример #16
0
 double PathToTarget(Vector2D * targ)
 {
     bool isLeft = false;
     double ang = AngleToTarget(targ);
     if (ang > 0) isLeft = true; // else false
     
     double theta;
     
     if (isLeft)
     {
         theta = position->getZ() + PI / 2.0;
     }
     else
     {
         theta = position->getZ() - PI / 2.0;
     }
     
     Vector2D * P = new Vector2D(position->getX(), position->getY());
     Vector2D * n = new Vector2D(r * cos(theta), r * sin(theta));
     
                 // R = targ - (P + n)
     Vector2D * G = P->add(n);
     Vector2D * R = targ->extract(G);
     Vector2D * ni = n->inv();
     
     double beta = R->angleTo(n->inv());
     double beta1 = ni->anticlockAngle(R);
     if (!isLeft) beta1 *= -1;
     double e = acos(r / R->norm());
     
     if (beta1 < 0)
         beta = 2 * PI - beta;
     
     /*
     if (beta1 > 0 && !isLeft)
         beta = 2 * PI - beta;
     */
      
     double eps = beta - e;
     double L = r * eps;
     
     double Rn = R->norm();
     double T = sqrt(Rn * Rn + r * r);
     return L + T;
 }
Пример #17
0
bool AlignNeuronsCommand::undoCommand() {
	if(mNeuronsToAlign.empty() || mVisualizationContext == 0) {
		return false;
	}	
	SimpleNetworkVisualizationHandler *handler = dynamic_cast<SimpleNetworkVisualizationHandler*>(
			mVisualizationContext->getVisualizationHandler());

	if(handler == 0) {
		return false;
	}
	QMutexLocker guard(mVisualizationContext->getSelectionMutex());

	NeuralNetwork *network = handler->getNeuralNetwork();

	if(network == 0) {
		return false;
	}

	
	for(int i = 0; i < mNeuronsToAlign.size() && i < mPreviousPositions.size(); ++i) {
		NeuralNetworkElement *neuron = mNeuronsToAlign.at(i);
		Vector3D pos = mPreviousPositions.at(i);

		if(neuron == 0) {
			continue;
		}
		if(dynamic_cast<NeuroModule*>(neuron) != 0) {
			NeuralNetworkUtil::moveNeuroModuleTo(dynamic_cast<NeuroModule*>(neuron),
												 pos.getX(), pos.getY(), pos.getZ());
		}
		else {
			neuron->setPosition(pos);
		}
	}
	
	//handler->rebuildView();
	Neuro::getNeuralNetworkManager()->triggerNetworkStructureChangedEvent();
	//mVisualizationContext->notifyNeuralNetworkModified();

	return true;
}
Пример #18
0
bool UVSphere::hit(const Ray3D& r, float tmin, float tmax, float time, HitRecord& record) const
{
	Vector3D temp = r.getOrigin() - center;

	double a = dotProduct(r.getDirection(), r.getDirection());
	double b = 2*dotProduct(r.getDirection(), temp);
	double c = dotProduct(temp, temp) - radius*radius;

	double disc = b*b -4*a*c;
	//is there some intersection

	if(disc > 0.0) {
		disc = sqrt(disc);
		double t = (-b - disc) / (2.0*a);
		if (t < tmin)
			t = (-b + disc) /(2.0*a);
		if (t < tmin || t > tmax)
			return false;

		record.t = t;
		record.p = record.texp = (r.getOrigin() + t*r.getDirection());
		record.uvw.initFromW((record.p - center) / radius);

		Vector3D n = (record.p - center) / radius;
		float twopi = 6.28318530718f;
		float theta = acos(n.getZ());
		float phi = atan2(n.getY(), n.getX());
		if (phi < 0.0f) phi+= twopi;

		float one_over_2pi = .159154943029f;
		float one_over_pi = .318309886184f;
		float pi = 3.14159;
		record.uv = Vector2D(phi*one_over_2pi, (pi-theta)*one_over_pi);

		record.mat_ptr = material;
		return true;
	}
	return false;
}
Пример #19
0
Vector3D Vector3D::getVectorTo(const Vector3D &other) {
	Length x = other.getX().minus(_x);
	Length y = other.getY().minus(_y);
	Length z = other.getZ().minus(_z);
	return Vector3D(x, y, z);
}
Пример #20
0
Vector3D Matrix4::operator*(Vector3D v)
{
	return Vector3D(v.getX()*mat[0][0]+ v.getY()*mat[0][1] + v.getZ()*mat[0][2] + mat[0][3],
		v.getX()*mat[1][0]+ v.getY()*mat[1][1] + v.getZ()*mat[1][2] + mat[1][3],
		v.getX()*mat[2][0]+ v.getY()*mat[2][1] + v.getZ()*mat[2][2] + mat[2][3]);
}
Пример #21
0
void StickyObj::setCollidingPointPos(int i, Vector3D pos){
	dVector3 temp;
	dBodyVectorFromWorld(bodyID,pos.getX(),pos.getY(),pos.getZ(),temp);
	collidingPointPos[i] = Vector3D(temp[0],temp[1],temp[2]);
}
dJointID ODE_PID_PassiveActuatorModel::createJoint(dBodyID body1, dBodyID body2) {

	Vector3DValue *jointAxisPoint1 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint1"));
	Vector3DValue *jointAxisPoint2 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint2"));
	DoubleValue *minAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MinAngle"));
	DoubleValue *maxAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MaxAngle"));

	if(jointAxisPoint1 == 0 || jointAxisPoint2 == 0 || minAngleValue == 0 || maxAngleValue == 0) {
		Core::log("ODE_PID_PassiveActuatorModel: Could not find all required parameter values.");
		return 0;
	}

	if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) {
		Core::log("ODE_PID_PassiveActuatorModel: Invalid axis points " + jointAxisPoint1->getValueAsString() + " and " + jointAxisPoint2->getValueAsString() + ".");
		return 0;
	}

	if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) {
		Core::log("Invalid axes for ODE_PID_PassiveActuatorModel.");
		return 0;
	}

	Vector3D anchor = jointAxisPoint1->get();
	Vector3D axis = jointAxisPoint2->get() - jointAxisPoint1->get();

	dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup);
	dJointAttach(joint, body1, body2);
	dJointSetAMotorMode(joint, dAMotorEuler);
	dJointSetAMotorParam(joint, dParamVel, 0.0);
	dJointSetAMotorParam(joint, dParamFMax, 1.0);
	dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get());
	dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get());
	dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get());
	dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get());
	dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get());

	axis.normalize();
	Vector3D perpedicular;
	if(axis.getY() != 0.0 || axis.getX() != 0.0) {
		perpedicular.set(-axis.getY(), axis.getX(), 0);
	}
	else {
		perpedicular.set(0, -axis.getZ(), axis.getY());
	}

	perpedicular.normalize();

	// If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added.
	if(body1 == 0) {
		dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ());
	}
	else {
		dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ());
	}
	if(body2 == 0) {
		dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(), 
			-perpedicular.getZ());
	}
	else {
		dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(), 
			perpedicular.getZ());
	}

	mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
	dJointAttach(mHingeJoint, body1, body2);

	dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ());
	dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ());

	double minAngle = (minAngleValue->get() * Math::PI) / 180.0;
	double maxAngle = (maxAngleValue->get() * Math::PI) / 180.0;

	if(body1 == 0) {
		double tmp = minAngle;
		minAngle = -1.0 * maxAngle;
		maxAngle = -1.0 * tmp;
	}

	dJointSetHingeParam(mHingeJoint, dParamLoStop, minAngle);
	dJointSetHingeParam(mHingeJoint, dParamHiStop, maxAngle);

	dJointSetHingeParam(mHingeJoint, dParamVel, 0.0);
	return joint;
}
Пример #23
0
Matrix4 Matrix4::fromTranslation(const Vector3D& Translation)
{
	return fromTranslation(Translation.getX(), Translation.getY(), Translation.getZ());
}
Пример #24
0
bool AlignNeuronsCommand::doCommand() {

	if(mNeuronsToAlign.size() < 2 || mVisualizationContext == 0 || mNeuronsToAlign.size() != mBoundingBoxes.size()) {
		return false;
	}	
	SimpleNetworkVisualizationHandler *handler = dynamic_cast<SimpleNetworkVisualizationHandler*>(
			mVisualizationContext->getVisualizationHandler());

	if(handler == 0) {
		return false;
	}
	QMutexLocker guard(mVisualizationContext->getSelectionMutex());

	ModularNeuralNetwork *network = handler->getNeuralNetwork();

	if(network == 0) {
		return false;
	}

	mPreviousPositions.clear();

	//calculate desired distance based on the positions of the upperleft elements.
	double distance = 1.0;
	double startingPos = 0.0;
	int secondStartingPos = 0.0;
	int indexOfFirst = 0;
	int indexOfSecond = 0;

	QList<NeuralNetworkElement*> mSortedNeurons = mNeuronsToAlign;

	if(mSortedNeurons.size() < 2) {
		return true;
	}
	
	QList<NeuralNetworkElement*> mOrderedNeurons;

	if(mType == TYPE_HORIZONTAL) {
		
		for(int i = 0; i < mSortedNeurons.size(); ++i) {
			NeuralNetworkElement *elem = mSortedNeurons.at(i);
			bool added = false;
			for(int j = 0; j < mOrderedNeurons.size(); ++j) {
				if(elem->getPosition().getX() < mOrderedNeurons.at(j)->getPosition().getX()) {
					mOrderedNeurons.insert(j, elem);
					added = true;
					break;
				}
			}
			if(!added) {
				mOrderedNeurons.append(elem);
			}
		}
		
		//TODO complete the spacing and distance handling!
		
		indexOfFirst = mOrderedNeurons.indexOf(mSortedNeurons.at(mSortedNeurons.size() - 1));
		indexOfSecond = mOrderedNeurons.indexOf(mSortedNeurons.at(mSortedNeurons.size() - 2));
		
		if(mMode == MODE_DISTANCE) {
			NeuralNetworkElement *lastElem1 = mSortedNeurons.at(mSortedNeurons.size() - 1);
			NeuralNetworkElement *lastElem2 = mSortedNeurons.at(mSortedNeurons.size() - 2);
			
			//flip bounding boxes if neccessary
			if(lastElem1->getPosition().getX() < lastElem2->getPosition().getX()) {
				QRectF box1 = mBoundingBoxes.at(mBoundingBoxes.size() - 1);
				QRectF box2 = mBoundingBoxes.at(mBoundingBoxes.size() - 2);
				
				startingPos = ((box1.right() - box1.left()) / 2.0) + box1.left();
				secondStartingPos = ((box2.right() - box2.left()) / 2.0) + box2.left();
				distance = (((box2.right() - box2.left()) / 2.0) + box2.left()) - startingPos;
				
				//cerr << "first less " << startingPos << " " << secondStartingPos << " d: " << distance << " ix1: " << indexOfFirst << " ix2: " << indexOfSecond << endl;
			}
			else {
				QRectF box1 = mBoundingBoxes.at(mBoundingBoxes.size() - 1);
				QRectF box2 = mBoundingBoxes.at(mBoundingBoxes.size() - 2);

				startingPos = ((box1.right() - box1.left()) / 2.0) + box1.left();
				secondStartingPos = ((box2.right() - box2.left()) / 2.0) + box2.left();
				distance = startingPos - (((box2.right() - box2.left()) / 2.0) + box2.left());
				
				//cerr << "second less " << startingPos << " " << secondStartingPos << " d: " << distance << " ix1: " << indexOfFirst << " ix2: " << indexOfSecond << endl;
			}
			
		}
		else if(mMode == MODE_SPACING) {
			//TODO
// 			QRectF box1 = mBoundingBoxes.at(mBoundingBoxes.size() - 1);
// 			QRectF box2 = mBoundingBoxes.at(mBoundingBoxes.size() - 2);
// 			startingPos = ((box1.right() - box1.left()) / 2.0) + box1.left();
// 			distance = box2.left() - box1.right();
		}
		else if(mMode == MODE_CENTER) {
			startingPos = mSortedNeurons.at(mSortedNeurons.size() - 1)->getPosition().getY();
		}
		else if(mMode == MODE_TOP) {
			startingPos = mBoundingBoxes.at(mSortedNeurons.size() - 1).top();
		}
		else if(mMode == MODE_BOTTOM) {
			startingPos = mBoundingBoxes.at(mSortedNeurons.size() - 1).bottom();
		}
	}
	else {
		if(mMode == MODE_DISTANCE) {
			QRectF box1 = mBoundingBoxes.at(mBoundingBoxes.size() - 1);
			QRectF box2 = mBoundingBoxes.at(mBoundingBoxes.size() - 2);
			startingPos = ((box1.bottom() - box1.top()) / 2.0) + box1.top();
			distance = ((box2.bottom() - box2.top()) / 2.0) + box2.top() - startingPos;
		}
		else if(mMode == MODE_SPACING) {
			//TODO
		}
		else if(mMode == MODE_CENTER) {
			startingPos = mSortedNeurons.at(mSortedNeurons.size() - 1)->getPosition().getX();
		}
		else if(mMode == MODE_LEFT) {
			startingPos = mBoundingBoxes.at(mSortedNeurons.size() - 1).left();
		}
		else if(mMode == MODE_RIGHT) {
			startingPos = mBoundingBoxes.at(mSortedNeurons.size() - 1).right();
		}
	}

	//mNeuronsToAlign = mSortedNeurons;

	for(int i = 0; i < mNeuronsToAlign.size(); ++i) {
		NeuralNetworkElement *neuron = mNeuronsToAlign.at(i);
		QRectF box = mBoundingBoxes.at(i);

		if(neuron == 0) {
			continue;
		}

		mPreviousPositions.append(neuron->getPosition());
		Vector3D pos = neuron->getPosition();
		if(mType == TYPE_HORIZONTAL) {
			if(mMode == MODE_DISTANCE) {
// 				pos.setX(startingPos + ((mNeuronsToAlign.size() - 1 - i) * distance) 
// 				+ (pos.getX() - (box.right() - box.left()) / 2.0) - box.left());


				int index = mOrderedNeurons.indexOf(neuron);
				
				//cerr << "getting pos " << index << " and " << indexOfFirst << " , " << indexOfSecond << " dist: " << distance << " p1 " << startingPos << " p2 " << secondStartingPos << endl;
				
				if(index < 0 || index > mOrderedNeurons.size()) {
					//cerr << "index was: " << index << endl;
					continue;
				}
				
				if(index < indexOfFirst && index < indexOfSecond) {
					pos.setX(startingPos + ((index - indexOfFirst) * distance) 
					+ (pos.getX() - (box.right() - box.left()) / 2.0) - box.left());
				}
				else if(index > indexOfFirst && index > indexOfSecond) {
					pos.setX(secondStartingPos - ((indexOfSecond - index) * distance) 
					+ (pos.getX() - (box.right() - box.left()) / 2.0) - box.left());
				}
			}
			else if(mMode == MODE_SPACING) {
// 				cerr << "Startingpos: " << startingPos << " and dist " << distance << " pos obj: " << pos.getX() << endl;
// 				pos.setX(startingPos 
// 						- (pos.getX() - (box.right() - box.left()) / 2.0) - box.left());
// 				startingPos -= (box.width() + distance);
				
			}
			else if(mMode == MODE_CENTER) {
				pos.setY(startingPos);
			}
			else if(mMode == MODE_TOP) {
				pos.setY(startingPos + (pos.getY() - box.top()));
			}
			else if(mMode == MODE_BOTTOM) {
				pos.setY(startingPos + (pos.getY() - box.bottom()));
			}
		}
		else {
			if(mMode == MODE_DISTANCE) {
				//pos.setY(startingPos + (i * distance));
				pos.setY(startingPos + ((mNeuronsToAlign.size() - 1 - i) * distance) 
						+ (pos.getY() - (box.bottom() - box.top()) / 2.0) - box.top());
			}
			else if(mMode == MODE_SPACING) {
				//TODO
			}
			else if(mMode == MODE_CENTER) {
				pos.setX(startingPos);
			}
			else if(mMode == MODE_LEFT) {
				pos.setX(startingPos + (pos.getX() - box.left()));
			}
			else if(mMode == MODE_RIGHT) {
				pos.setX(startingPos + (pos.getX() - box.right()));
			}
			
		}
		if(dynamic_cast<NeuroModule*>(neuron) != 0) {
			NeuralNetworkUtil::moveNeuroModuleTo(dynamic_cast<NeuroModule*>(neuron),
												 pos.getX(), pos.getY(), pos.getZ());
		}
		else {
			neuron->setPosition(pos);
		}
	}

	//handler->rebuildView();
	Neuro::getNeuralNetworkManager()->triggerNetworkStructureChangedEvent();
	//mVisualizationContext->notifyNeuralNetworkModified();

	return true;
}
Пример #25
0
Point3D Point3D::operator-=(const Vector3D &p)
{
	increment(-p.getX(), -p.getY(), -p.getZ());
	return *this;
}
Пример #26
0
int main() {
	SDL_Init(SDL_INIT_VIDEO);

	SDL_Surface *screen = SDL_SetVideoMode(1024, 768, 32, SDL_HWSURFACE);
	SDL_Surface *trace = SDL_CreateRGBSurface(SDL_HWSURFACE, 1024, 768, 32, 0, 0, 0, 0);
	SDL_WM_SetCaption("Solar system demo", NULL);

	Uint32 white = SDL_MapRGB(screen->format, 255, 255, 255);
	Uint32 black = SDL_MapRGB(screen->format, 0, 0, 0);
	Uint32 red = SDL_MapRGB(screen->format, 255, 0, 0);
    Uint32 green = SDL_MapRGB(screen->format, 0, 255, 0);
    Uint32 blue = SDL_MapRGB(screen->format, 0, 0, 255);

	// Universe U;
	// Sun *sun = new Sun(1e+10);
	// U.addBody(sun);
	// U.addBody(new SpaceBody(0, sun, 0.7, 150));
	// U.addBody(new SpaceBody(0, sun, 0.5, 150, 0, 0, M_PI/4));
	Universe U;
	double sunmass = 1e+10;
	StaticIntegrator si;
	U.addBody(new Body(sunmass, si));

	KeplerIntegrator ki1(Orbit(sunmass, 0, 0.7, 150));
	U.addBody(new Body(0, ki1));

	// KeplerIntegrator ki2(Orbit(sunmass, 0, 0, 150, M_PI/2));
	// U.addBody(new Body(0, ki2));

    EulerIntegrator ei(Vector3D(150, 0, 0), Vector3D(0, -sqrt((sunmass * G)/150), 0));
    Body *eulerbody = new Body(0, ei);
    U.addBody(eulerbody);

	LeapfrogIntegrator lfi(Vector3D(150, 0, 0), Vector3D(0, -sqrt((sunmass * G)/150), 0));
	Body *leapfrogbody = new Body(0, lfi);
	U.addBody(leapfrogbody);


	SDL_Rect pos1, pos2;
	pos2.x = pos2.y = 0;
	Vector3D v;
	SDL_Surface *rect = SDL_CreateRGBSurface(SDL_HWSURFACE, 10, 10, 32, 0, 0, 0, 0);
	SDL_FillRect(rect, NULL, white);
	SDL_Surface *littlerect = SDL_CreateRGBSurface(SDL_HWSURFACE, 1, 1, 32, 0, 0, 0, 0);

	bool ok = true;
	SDL_Event evt;
    Uint32 color;
    Uint32 startTime = SDL_GetTicks();
	while (ok) {
		SDL_PollEvent(&evt);
		switch (evt.type) {
			case SDL_QUIT:
				ok = false;
				break;
		}

		SDL_FillRect(screen, NULL, black);
		SDL_BlitSurface(trace, NULL, screen, &pos2);

		// for(int i=0;i<2000;i++)
            U.evolve(200);
		for (std::shared_ptr<Body> &it : U.getBodies()) {
            color=it.get() == eulerbody ? green : (it.get() == leapfrogbody ? red : blue);
			v = Vector3D(512, 384, 0) + it->getPosition();
            if(it.get() == leapfrogbody)
                v+=Vector3D(2,0,0);
			pos1.x = v.getX();
            pos1.y = v.getY();
            SDL_FillRect(littlerect, NULL, color);
            SDL_BlitSurface(littlerect, NULL, trace, &pos1);
            pos1.x -= 5;
            pos1.y -= 5;
			SDL_FillRect(rect, NULL, color);
            SDL_BlitSurface(rect, NULL, screen, &pos1);
		}

		SDL_Flip(screen);
	}

	SDL_Quit();

	return EXIT_SUCCESS;
}
void Virtual3dCharacterViewer::drawSingleColorCube(Vector3D Color)
{
    GLfloat x1 = leftBottomX + xLength ;
    GLfloat y1 = leftBottomY + yLength ;
    GLfloat z1 = leftBottomZ ;

    GLfloat x2 = leftBottomX ;
    GLfloat y2 = leftBottomY + yLength ;
    GLfloat z2 = leftBottomZ ;

    GLfloat x3 = leftBottomX ;
    GLfloat y3 = leftBottomY + yLength ;
    GLfloat z3 = leftBottomZ + zLength ;

    GLfloat x4 = leftBottomX + xLength ;
    GLfloat y4 = leftBottomY + yLength ;
    GLfloat z4 = leftBottomZ + zLength ;

    GLfloat x5 = leftBottomX + xLength ;
    GLfloat y5 = leftBottomY ;
    GLfloat z5 = leftBottomZ + zLength ;

    GLfloat x6 = leftBottomX ;
    GLfloat y6 = leftBottomY ;
    GLfloat z6 = leftBottomZ + zLength ;

    GLfloat x7 = leftBottomX ;
    GLfloat y7 = leftBottomY ;
    GLfloat z7 = leftBottomZ ;

    GLfloat x8 = leftBottomX + xLength ;
    GLfloat y8 = leftBottomY ;
    GLfloat z8 = leftBottomZ ;

    GLfloat normalX , normalY , normalZ ;

    // 开始画立方体
    glDisable( GL_TEXTURE_2D );

    glBegin( GL_QUADS );

//    glColor3f( 0.0, 0.0, 1.0 );


        // 立方体的顶面
        getNormal ( x2-x1, y2-y1, z2-z1 ,
                    x3-x2, y3-y2, z3-z2 ,
                    &normalX , &normalY , &normalZ ) ;
        glNormal3f( normalX, normalY, normalZ );
 //       glColor3f( 0.0, 1.0, 0.0 );
        glColor3f( Color.getX(), Color.getY(), Color.getZ() );
        glVertex3f(  x1,  y1 , z1 );
        glVertex3f(  x2,  y2 , z2 );
        glVertex3f(  x3,  y3 , z3 );
        glVertex3f(  x4,  y4 , z4 );


        // 立方体的底面
        getNormal ( x6-x5, y6-y5, z6-z5 ,
                    x7-x6, y7-y6, z7-z6 ,
                    &normalX , &normalY , &normalZ ) ;
        glNormal3f( normalX, normalY, normalZ );
//        glColor3f( 1.0, 0.5, 0.0 );
        glVertex3f(  x5,  y5 , z5 );
        glVertex3f(  x6,  y6 , z6 );
        glVertex3f(  x7,  y7 , z7 );
        glVertex3f(  x8,  y8 , z8 );

        // 立方体的前面
        getNormal ( x3-x4, y3-y4, z3-z4 ,
                    x6-x3, y6-y3, z6-z3 ,
                    &normalX , &normalY , &normalZ ) ;
        glNormal3f( normalX, normalY, normalZ );
//        glColor3f( 1.0, 0.0, 0.0 );
        glVertex3f(  x4,  y4 , z4 );
        glVertex3f(  x3,  y3 , z3 );
        glVertex3f(  x6,  y6 , z6 );
        glVertex3f(  x5,  y5 , z5 );

        // 立方体的后面
        getNormal ( x7-x8, y7-y8, z7-z8 ,
                    x2-x7, y2-y7, z2-z7 ,
                    &normalX , &normalY , &normalZ ) ;
        glNormal3f( normalX, normalY, normalZ );
//        glColor3f( 0.5, 0.5, 0.0 );
        glVertex3f(  x8,  y8 , z8 );
        glVertex3f(  x7,  y7 , z7 );
        glVertex3f(  x2,  y2 , z2 );
        glVertex3f(  x1,  y1 , z1 );

        // 立方体的左面
        getNormal ( x2-x3, y2-y3, z2-z3 ,
                    x7-x2, y7-y2, z7-z2 ,
                    &normalX , &normalY , &normalZ ) ;
        glNormal3f( normalX, normalY, normalZ );
//        glColor3f( 0.0, 0.0, 1.0 );
        glVertex3f(  x3,  y3 , z3 );
        glVertex3f(  x2,  y2 , z2 );
        glVertex3f(  x7,  y7 , z7 );
        glVertex3f(  x6,  y6 , z6 );

        // 立方体的右面
        getNormal ( x4-x1, y4-y1, z4-z1 ,
                    x5-x4, y5-y4, z5-z4 ,
                    &normalX , &normalY , &normalZ ) ;
        glNormal3f( normalX, normalY, normalZ );
//        glColor3f( 1.0, 0.0, 1.0 );
        glVertex3f(  x1,  y1 , z1 );
        glVertex3f(  x4,  y4 , z4 );
        glVertex3f(  x5,  y5 , z5 );
        glVertex3f(  x8,  y8 , z8 );

    glEnd();

    if ( ENABLE_GL_TEXTURE_2D )
        glEnable( GL_TEXTURE_2D );
}
Пример #28
0
dJointID ODE_Dynamixel::createJoint(dBodyID body1, dBodyID body2) {

	if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get(), -1)) {
		Core::log("ODE_Dynamixel: Invalid axes for ODE_Dynamixel.", true);
		return 0;
	}

	Vector3D anchor = mJointAxisPoint1->get();
	Vector3D axis = mJointAxisPoint2->get() - mJointAxisPoint1->get();

	dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup);
	dJointAttach(joint, body1, body2);
	dJointSetAMotorMode(joint, dAMotorEuler);
	dJointSetAMotorParam(joint, dParamVel, 0.0);
	dJointSetAMotorParam(joint, dParamFMax, mDesiredMotorTorqueValue->get());
	dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get());
	dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get());
	dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get());
	dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get());
	dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get());

	axis.normalize();
	Vector3D perpedicular;
	if(axis.getY() != 0.0 || axis.getX() != 0.0) {
		perpedicular.set(-axis.getY(), axis.getX(), 0);
	}
	else {
		perpedicular.set(0, -axis.getZ(), axis.getY());
	}

	perpedicular.normalize();

	// If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added.
	if(body1 == 0) {
		dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ());
	}
	else {
		dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ());
	}
	if(body2 == 0) {
		dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(), 
			-perpedicular.getZ());
	}
	else {
		dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(), 
			perpedicular.getZ());
	}

	mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
	dJointAttach(mHingeJoint, body1, body2);

	dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ());
	dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ());

	if(body1 == 0) {
		double tmp = mMinAngle;
		mMinAngle = -1.0 * mMaxAngle;
		mMaxAngle = -1.0 * tmp;
	}

	dJointSetHingeParam(mHingeJoint, dParamLoStop, mMinAngle);
	dJointSetHingeParam(mHingeJoint, dParamHiStop, mMaxAngle);

	dJointSetHingeParam(mHingeJoint, dParamVel, 0.0);
	return joint;
}
Пример #29
0
Vector3D::Vector3D(const Vector3D &vec) {
	_x = vec.getX();
	_y = vec.getY();
	_z = vec.getZ();
}
Пример #30
0
Vector3D Vector3D::move(const Vector3D &movement) {
	Length x = movement.getX().plus(_x);
	Length y = movement.getY().plus(_y);
	Length z = movement.getZ().plus(_z);
	return Vector3D(x, y, z);
}