예제 #1
0
void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
{
  if (!g) return;
  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCCylinderClass) {
    dReal radius,length;
    dGeomCCylinderGetParams (g,&radius,&length);
    dsDrawCappedCylinder (pos,R,length,radius);
  }
  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom (g);
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331 (actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);
    drawGeom (g2,actual_pos,actual_R);
  }
}
예제 #2
0
/**
 * @brief Desenhar o campo da simulacao
 *
 * Desenha toda a parte grafica da simulacao, como
 * as paredes, as linhas, os robos e a bola.
 *
 */
void desenharCampo()
{
    dVector3 ss;
    dsSetColor (0,0,0);

    for(int i=0; i < 6; i++) //Paredes
    {
        dGeomBoxGetLengths (wall[i],ss);
        dsDrawBox (dGeomGetPosition(wall[i]), dGeomGetRotation(wall[i]), ss);
    }

    for(int i=0; i < 3;i++) //Gols
    {
        dGeomBoxGetLengths (goalL[i],ss);
        dsDrawBox (dGeomGetPosition(goalL[i]), dGeomGetRotation(goalL[i]), ss);
        dGeomBoxGetLengths (goalR[i],ss);
        dsDrawBox (dGeomGetPosition(goalR[i]), dGeomGetRotation(goalR[i]), ss);
    }

    dsSetColor (1,1,1); //Círculo Central
    dsDrawCylinder(dGeomGetPosition(circle),dGeomGetRotation(circle),0.0001,CIRCLE_RADIUS);

    for (int i=0; i < 4; i++) //Quinas
    {
        dGeomBoxGetLengths (triangle[i],ss);
        dsDrawBox (dGeomGetPosition(triangle[i]), dGeomGetRotation(triangle[i]), ss);
    }
}
예제 #3
0
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
	if (!draw_geom){
		return;
	}

  if (!g) return;
  if (!pos) pos = dGeomGetPosition (g);
  if (!R) R = dGeomGetRotation (g);

  int type = dGeomGetClass (g);
  if (type == dBoxClass) {
    dVector3 sides;
    dGeomBoxGetLengths (g,sides);
    dsDrawBox (pos,R,sides);
  }
  else if (type == dSphereClass) {
    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dCapsuleClass) {
    dReal radius,length;
    dGeomCapsuleGetParams (g,&radius,&length);
    dsDrawCapsule (pos,R,length,radius);
  }
/*
  // cylinder option not yet implemented
  else if (type == dCylinderClass) {
    dReal radius,length;
    dGeomCylinderGetParams (g,&radius,&length);
    dsDrawCylinder (pos,R,length,radius);
  }
*/
  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom (g);
    const dReal *pos2 = dGeomGetPosition (g2);
    const dReal *R2 = dGeomGetRotation (g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331 (actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);
    drawGeom (g2,actual_pos,actual_R,0);
  }

  if (show_aabb) {
    // draw the bounding box for this geom
    dReal aabb[6];
    dGeomGetAABB (g,aabb);
    dVector3 bbpos;
    for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
    dVector3 bbsides;
    for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
    dMatrix3 RI;
    dRSetIdentity (RI);
    dsSetColorAlpha (1,0,0,0.5);
    dsDrawBox (bbpos,RI,bbsides);
  }
}
// simulation loop
static void simLoop (int pause)
{
    const dReal *pos;
    const dReal *R;
    // force for the spheres
  
    // find collisions and add contact joints
    dSpaceCollide (space,0,&nearCallback);
    // step the simulation
    dWorldQuickStep (world,0.01);  
    // remove all contact joints
    dJointGroupEmpty (contactgroup);
    // redraw sphere at new location
    pos = dGeomGetPosition (sphere0_geom);
    R = dGeomGetRotation (sphere0_geom);
    dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere0_geom));
     
    pos = dGeomGetPosition (sphere1_geom);
    R = dGeomGetRotation (sphere1_geom);
    dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere1_geom));

    pos = dGeomGetPosition (sphere2_geom);
    R = dGeomGetRotation (sphere2_geom);
    dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere2_geom));
}
예제 #5
0
bool wheelCallBack(dGeomID o1,dGeomID o2,PSurface* s)
{
    //s->id2 is ground
    const dReal* r; //wheels rotation matrix
    const dReal* p; //wheels rotation matrix
    if ((o1==s->id1) && (o2==s->id2))
    {
        r=dBodyGetRotation(dGeomGetBody(o1));
        p=dGeomGetPosition(o1);
    }
    if ((o1==s->id2) && (o2==s->id1))
    {
        r=dBodyGetRotation(dGeomGetBody(o2));
        p=dGeomGetPosition(o2);
    }

    s->surface.mode = dContactFDir1 | dContactMu2  | dContactApprox1 | dContactSoftCFM;
    s->surface.mu = fric(_w->cfg->robotSettings.WheelPerpendicularFriction);
    s->surface.mu2 = fric(_w->cfg->robotSettings.WheelTangentFriction);
    s->surface.soft_cfm = 0.002;

    dVector3 v={0,0,1,1};
    dVector3 axis;
    dMultiply0(axis,r,v,4,3,1);
    float l = sqrt(axis[0]*axis[0] + axis[1]*axis[1]);
    s->fdir1[0] = axis[0]/l;
    s->fdir1[1] = axis[1]/l;
    s->fdir1[2] = 0;
    s->fdir1[3] = 0;
    s->usefdir1 = true;
    return true;
}
예제 #6
0
void CODEGeom::get_global_form_bt(Fmatrix& form)
{
	dMULTIPLY0_331 ((dReal*)(&form.c),dGeomGetRotation(m_geom_transform),dGeomGetPosition(geom()));
	form.c.add(*((const Fvector*)dGeomGetPosition(m_geom_transform)));
	dMULTIPLY3_333 ((dReal*)(&form),dGeomGetRotation(m_geom_transform),dGeomGetRotation(geom()));
	//PHDynamicData::DMXtoFMX((dReal*)(&form),form);
}
예제 #7
0
void CODEGeom::get_global_center_bt(Fvector& center)
{
	center.set(*((const Fvector*)dGeomGetPosition(m_geom_transform)));
	dVector3 add;
	dMULTIPLY0_331 (add,dGeomGetRotation(m_geom_transform),dGeomGetPosition(geom()));
	center.x += add[0];
	center.y += add[1];
	center.z += add[2];
}
/*******************************************************************************
Function to draw a geometry object.
*******************************************************************************/
void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const
{
	if( !g )		//If the geometry object is missing, end the function.
		return;

	if( !position )	//Position was not passed?
		position = dGeomGetPosition( g );		//Then, get the geometry position.

	if( !orientation )	//Orientation was not given?
		orientation = dGeomGetRotation( g );	//And get existing geometry orientation.

	int type = dGeomGetClass( g );				//Get the type of geometry.
	
	if( type == dBoxClass )						//Is it a box?
	{
		dReal sides[3];
		dGeomBoxGetLengths( g, sides );				//Get length of sides.
		renderBox( sides, position, orientation );	//Render the actual box in environment.
	}

	if( type == dSphereClass )					//Is it a sphere?
	{
		dReal radius;
		radius = dGeomSphereGetRadius( g );				//Get the radius.
		renderSphere( radius, position, orientation );	//Render sphere in environment.
	}

	if( type == dCapsuleClass )
	{
		dReal radius;
		dReal length;
		dGeomCapsuleGetParams( g, &radius, &length );	//Get both radius and length.
		renderCapsule( radius, length, position, orientation );	//Render capsule in environment.
	}

	if( type == dGeomTransformClass )					//Is it an embeded geom in a composite body.
	{
		dGeomID g2 = dGeomTransformGetGeom( g );		//Get the actual geometry inside the wrapper.
		const dReal *position2 = dGeomGetPosition( g2 );	//Get position and orientation of wrapped geometry.
		const dReal *orientation2 = dGeomGetRotation( g2 );
		
		dVector3 actualPosition;						//Real world coordinated position and orientation
		dMatrix3 actualOrientation;						//of the wrapped geometry.
		
		dMultiply0_331( actualPosition, orientation, position2 );	//Get world coordinates of geometry position.
		actualPosition[0] += position[0];
		actualPosition[1] += position[1];
		actualPosition[2] += position[2];

		dMultiply0_333( actualOrientation, orientation, orientation2 );	//Get world coordinates of geom orientation.

		drawGeom( g2, actualPosition, actualOrientation );	//Draw embeded geometry.
	}
}
예제 #9
0
static void computeFinalTx(dGeomID geom_transform,dReal* final_pos,dReal* final_R)
{
	R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform");
	dGeomID obj=dGeomTransformGetGeom(geom_transform);
	const	dReal	*R	=dGeomGetRotation(geom_transform);
	const	dReal	*pos=dGeomGetPosition(geom_transform);
	dMULTIPLY0_331 (final_pos,R,dGeomGetPosition(obj));
	final_pos[0] += pos[0];
	final_pos[1] += pos[1];
	final_pos[2] += pos[2];
	dMULTIPLY0_333 (final_R,R,dGeomGetRotation(obj));
}
예제 #10
0
void simLoop (int pause)
{
  static bool DumpInfo=true;
  const dReal ss[3] = {0.02,0.02,0.02};
  dContactGeom contacts[8];
  int contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom));
  //fprintf(stdout,"Contact Count %d\n",contactcount);
  const dReal* pos;
  const dReal* R;
  dsSetTexture (DS_WOOD);
  pos = dGeomGetPosition (geoms[0]);
  R = dGeomGetRotation (geoms[0]);
  dsSetColor (0.6f,0.6f,1);
  dsDrawConvex(pos,R,planes,
	       planecount,
	       points,
	       pointcount,
	       polygons);
  pos = dGeomGetPosition (geoms[1]);
  R = dGeomGetRotation (geoms[1]);
  dsSetColor (0.4f,1,1);
  dsDrawConvex(pos,R,planes,
	       planecount,
	       points,
	       pointcount,
	       polygons);
  /*if (show_contacts) */
  dMatrix3 RI;
  dRSetIdentity (RI);
  dsSetColor (1.0f,0,0);
  for(int i=0;i<contactcount;++i)
    {
      if(DumpInfo)
	{
	  //DumpInfo=false;
	  fprintf(stdout,"Contact %d Normal %f,%f,%f Depth %f\n",
		  i,
		  contacts[i].normal[0],
		  contacts[i].normal[1],
		  contacts[i].normal[2],
		  contacts[i].depth);
	}
      dsDrawBox (contacts[i].pos,RI,ss);
    }
  if(DumpInfo)
    DumpInfo=false;

}
예제 #11
0
    void cPhysicsObject::Update(durationms_t currentTime)
    {
      if (bDynamic) {
        const dReal* p0 = nullptr;
        const dReal* r0 = nullptr;

        dQuaternion q;

        if (bBody) {
          p0 = dBodyGetPosition(body);
          r0 = dBodyGetQuaternion(body);
          const dReal* v0 = dBodyGetLinearVel(body);
          //const dReal *a0=dBodyGetAngularVel(body);

          v[0] = v0[0];
          v[1] = v0[1];
          v[2] = v0[2];
        } else {
          p0 = dGeomGetPosition(geom);
          dGeomGetQuaternion(geom, q);
          r0 = q;

          // These are static for the moment
          v[0] = 0.0f;
          v[1] = 0.0f;
          v[2] = 0.0f;
        }

        ASSERT(p0 != nullptr);
        ASSERT(r0 != nullptr);
        position.Set(p0[0], p0[1], p0[2]);
        rotation.SetFromODEQuaternion(r0);
      }
    }
예제 #12
0
파일: main.cpp 프로젝트: bmarcott/cs275
void drawGeom( dGeomID g, int colored = 0 )
{
	if( !g )		//If the geometry object is missing, end the function.
		return;

	const dReal *position;		//Define pointers to internal positions and orientations.
	const dReal *orientation;	//Pointers to constant objects (so the objects will not change).

	int type = dGeomGetClass( g );				//Get the type of geometry.

	position = dGeomGetPosition( g );		//Then, get the geometry position.
	orientation = dGeomGetRotation( g );	//And get existing geometry orientation.
	
	if( type == dBoxClass )						//Is it a box?
	{		
		dReal sides[3];
		dGeomBoxGetLengths( g, sides );				//Get length of sides.
		renderBox( sides, position, orientation, colored );	//Render the actual box in environment.
	}
	else if (type == dCylinderClass)
	{
		dReal radius, length;
		dGeomCylinderGetParams(g, &radius, &length);
		renderCylinder(radius, length, position, orientation);
	}
	else if (type == dCapsuleClass)
	{
		dReal radius, length;
		dGeomCapsuleGetParams(g, &radius, &length);
		renderCapsule(radius, length, position, orientation);
	}
}
예제 #13
0
Matrix PhysicsGeom::getTransformation(void) const
{
    Matrix Transformation;
    
    Vec3f Translation;
    const dReal* t = dGeomGetOffsetPosition(_GeomID);
    Translation.setValues( t[0],t[1],t[2] );

    dQuaternion q;
    dGeomGetOffsetQuaternion(_GeomID, q);
    Quaternion Rotation;
    Rotation.setValueAsQuat(q[1], q[2], q[3], q[0]);

    Transformation.setTransform(Translation,Rotation);

    if(isPlaceable())
    {

        t = dGeomGetPosition(_GeomID);
        Translation.setValues( t[0],t[1],t[2] );

        dGeomGetQuaternion(_GeomID, q);
        Rotation.setValueAsQuat(q[1], q[2], q[3], q[0]);

        Matrix NonBodyTransformation;
        NonBodyTransformation.setTransform(Translation,Rotation);
        Transformation.mult(NonBodyTransformation);
    }

    return Transformation;
}
예제 #14
0
파일: Solver.cpp 프로젝트: fathat/game-src
void DynamicsSolver::GetTransform ( dGeomID geom, Matrix* M )
{
	float outmatrix[16];
	dReal* L; dReal* R;
	L  = (dReal*)dGeomGetPosition(geom);
	R  = (dReal*)dGeomGetRotation(geom);

	outmatrix[0]=(float)R[0];
	outmatrix[1]=(float)R[4];
	outmatrix[2]=(float)R[8];
	outmatrix[3]=0;
	outmatrix[4]=(float)R[1];
	outmatrix[5]=(float)R[5];
	outmatrix[6]=(float)R[9];
	outmatrix[7]=0;
	outmatrix[8]=(float)R[2];
	outmatrix[9]=(float)R[6];
	outmatrix[10]=(float)R[10];
	outmatrix[11]=0;
	outmatrix[12]=(float)L[0];
	outmatrix[13]=(float)L[1];
	outmatrix[14]=(float)L[2];
	outmatrix[15]=1;
	memcpy( M->Mat, outmatrix, sizeof(float) * 16);
}
예제 #15
0
파일: base.cpp 프로젝트: cxindex/gtglos
void base::passive_square_render (void)
{ //without any animation, but with set-type img
	if(body) odepos=dBodyGetPosition(body);
	else odepos=dGeomGetPosition(geom);
	x=odepos[0]-texture[0].w/2; //is it needed?
	y=odepos[1]-texture[0].h/2; //here maybe some troubles with up
	
	glMatrixMode(GL_TEXTURE);
	glLoadIdentity();

	switch(texture[last].type)
	{
		case 0:
			glScalef((float)1*texture[last].n, (float)1*texture[last].n, 1.0f); //1==fullsize. -(w+1/n)==one frame size + -- invers
			break;
		case 1:
			glScalef((float)1*texture[last].n, (float)1/texture[last].n, 1.0f); //1==fullsize. -(w+1/n)==one frame size + -- invers
			break;
		case 2:
			glScalef((float)1/texture[last].n, (float)1/texture[last].n, 1.0f); //1==fullsize. -(w+1/n)==one frame size + -- invers
			break;
		case 3:
			glScalef((float)1/texture[last].n, (float)1*texture[last].n, 1.0f); //1==fullsize. -(w+1/n)==one frame size + -- invers
			break;
	}

	//~ glScalef((float)1/texture[last].w, (float)1*texture[last].n, 1.0f);

	glBindTexture( GL_TEXTURE_2D, texture[0].texture );
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	//---------------------------------
	rotate();
	glTranslated(x, y, 0);	//set position
	glTranslated(texture[last].w/2, texture[last].h/2, 0); //all three for rotate
	glRotatef(rotate_angle,0,0,1);
	glTranslated(-texture[last].w/2, -texture[last].h/2, 0);

	glBegin( GL_QUADS );
	    glColor3f(1.0f,1.0f,1.0f);

		//Bottom-r vertex (corner)
		glTexCoord2i( 0, 1 );
		glVertex3f( 0.f, 0.f, 0.0f );
		
		//Bottom-l vertex (corner)
		glTexCoord2i( 1,1 );
		glVertex3f( texture[last].w, 0.f, 0.f );
		
		//Top-l vertex (corner)
		glTexCoord2i( 1, 0 );
		glVertex3f( texture[last].w, texture[last].h, 0.f );
		
		//Top-r vertex (corner)
		glTexCoord2i( 0,0 );
		glVertex3f( 0.f, texture[last].h, 0.f );
	glEnd();
	if(body) hack_2d();
}
void ApproxDistanceSensor::DistanceSensor::updateValue()
{
  pose = physicalObject->pose;
  pose.conc(offset);
  invertedPose = pose.invert();
  Vector3<> boxPos = pose * Vector3<>(max * 0.5f, 0.f, 0.f);
  dGeomSetPosition(geom, boxPos.x, boxPos.y, boxPos.z);
  dMatrix3 matrix3;
  ODETools::convertMatrix(pose.rotation, matrix3);
  dGeomSetRotation(geom, matrix3);
  closestGeom = 0;
  closestSqrDistance = maxSqrDist;
  dSpaceCollide2(geom, (dGeomID)Simulation::simulation->movableSpace, this, (dNearCallback*)&staticCollisionWithSpaceCallback);
  dSpaceCollide2(geom, (dGeomID)Simulation::simulation->staticSpace, this, (dNearCallback*)&staticCollisionCallback);
  if(closestGeom)
  {
    const dReal* pos = dGeomGetPosition(closestGeom);
    Geometry* geometry = (Geometry*)dGeomGetData(closestGeom);
    data.floatValue = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - pose.translation).abs() - geometry->innerRadius;
    if(data.floatValue < min)
      data.floatValue = min;
  }
  else
    data.floatValue = max;
}
// Getting data
void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){
    dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");

    dxTriMesh* Geom = (dxTriMesh*)g;

    const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
    const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);

    dVector3 v[3];
	FetchTriangle(Geom, Index, Position, Rotation, v);

    if (v0){
        (*v0)[0] = v[0][0];
        (*v0)[1] = v[0][1];
        (*v0)[2] = v[0][2];
        (*v0)[3] = v[0][3];
    }
    if (v1){
        (*v1)[0] = v[1][0];
        (*v1)[1] = v[1][1];
        (*v1)[2] = v[1][2];
        (*v1)[3] = v[1][3];
    }
    if (v2){
        (*v2)[0] = v[2][0];
        (*v2)[1] = v[2][1];
        (*v2)[2] = v[2][2];
        (*v2)[3] = v[2][3];
    }
}
예제 #18
0
/*** ゴールの描画 ***/
static void drawGoal()
{
    dsSetTexture(DS_NONE);
    for (int i = 0; i < GOAL_PARTS_NUM; i++)
    {
        if (i < 4)   dsSetColor(1.3, 1.3, 1.3);
        else         dsSetColor(1.3, 1.3, 0.0);
        dsDrawBox(dGeomGetPosition(goal_parts[i]),
                  dGeomGetRotation(goal_parts[i]),GOAL_SIDES[i]);

        if (i < 4)   dsSetColor(1.3, 1.3, 1.3);
        else         dsSetColor(0.0, 0.0, 1.3);
        dsDrawBox(dGeomGetPosition(goal_parts2[i]),
                  dGeomGetRotation(goal_parts2[i]),GOAL_SIDES[i]);
    }
}
예제 #19
0
void DrawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
    // If the geom ID is missing then return immediately.
    if (!g)
    {
        return;
    }

    // If there was no position vector supplied then get the existing position.
    if (!pos)
    {
        pos = dGeomGetPosition (g);
    }

    // If there was no rotation matrix given then get the existing rotation.
    if (!R)
    {
        R = dGeomGetRotation (g);
    }

    // Get the geom's class type.
    int type = dGeomGetClass (g);

    if (type == dBoxClass)
    {
        // Create a temporary array of floats to hold the box dimensions.
        dReal sides[3];
        dGeomBoxGetLengths(g, sides);

        // Now to actually render the box we make a call to DrawBox, passing the geoms dimensions, position vector and
        // rotation matrix. And this function is the subject of our next discussion.
       // DrawBox(sides, pos, R);
    }
}
예제 #20
0
void CPHCollisionDamageReceiver::CollisionCallback(bool& do_colide,bool bo1,dContact& c,SGameMtl* material_1,SGameMtl* material_2)
{
	if(material_1->Flags.test(SGameMtl::flPassable)||material_2->Flags.test(SGameMtl::flPassable))return;
	dBodyID						b1					=	dGeomGetBody(c.geom.g1)	;
	dBodyID						b2					=	dGeomGetBody(c.geom.g2) ;
	dxGeomUserData				*ud_self			=	bo1 ? retrieveGeomUserData(c.geom.g1):retrieveGeomUserData(c.geom.g2);
	dxGeomUserData				*ud_damager			=	bo1 ? retrieveGeomUserData(c.geom.g2):retrieveGeomUserData(c.geom.g1);
	
	SGameMtl					*material_self		=	bo1 ? material_1:material_2;
	SGameMtl					*material_damager	=	bo1 ? material_2:material_1;
	VERIFY						(ud_self);
	CPhysicsShellHolder			*o_self			=	ud_self->ph_ref_object;
	CPhysicsShellHolder			*o_damager		=	NULL;if(ud_damager)o_damager=ud_damager->ph_ref_object;
	u16							source_id		=	o_damager ? o_damager->ID():u16(-1);
	CPHCollisionDamageReceiver	*dr	=o_self->PHCollisionDamageReceiver();
	VERIFY2(dr,"wrong callback");
	
	float damager_material_factor=material_damager->fBounceDamageFactor;

	if(ud_damager&&ud_damager->ph_object&&ud_damager->ph_object->CastType()==CPHObject::tpCharacter)
	{
		CCharacterPhysicsSupport* phs=o_damager->character_physics_support();
		if(phs->IsSpecificDamager())damager_material_factor=phs->BonceDamageFactor();
	}

	float dfs=(material_self->fBounceDamageFactor+damager_material_factor);
	if(fis_zero(dfs)) return;
	Fvector dir;dir.set(*(Fvector*)c.geom.normal);
	Fvector pos;
	pos.sub(*(Fvector*)c.geom.pos,*(Fvector*)dGeomGetPosition(bo1 ? c.geom.g1:c.geom.g2));//it is not true pos in bone space
	dr->Hit(source_id,ud_self->bone_id,E_NL(b1,b2,c.geom.normal)*damager_material_factor/dfs,dir,pos);
	
}
예제 #21
0
const osg::Vec3 BoxObstacle::getPosition() {
	if (box_!= 0) {
		const dReal* pos = dBodyGetPosition(box_);
		return osg::Vec3(pos[0], pos[1], pos[2]);
	}
	const dReal* pos = dGeomGetPosition(boxGeom_);
	return osg::Vec3(pos[0], pos[1], pos[2]);
}
void _InitCylinderTrimeshData(sData& cData)
{
    // get cylinder information
    // Rotation
    const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
    dMatrix3Copy(pRotCyc,cData.mCylinderRot);
    dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot);

    // Position
    const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
    dVector3Copy(*pPosCyc,cData.vCylinderPos);
    // Cylinder axis
    dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
    // get cylinder radius and size
    dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);

    // get trimesh position and orientation
    const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh);
    dMatrix3Copy(pRotTris,cData.mTrimeshRot);
    dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot);

    // Position
    const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh);
    dVector3Copy(*pPosTris,cData.vTrimeshPos);


    // calculate basic angle for 8-gon
    dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS;
    // calculate angle increment
    dReal fAngleIncrement = fAngle*REAL(2.0);

    // calculate plane normals
    // axis dependant code
    for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++)
    {
        cData.avCylinderNormals[i][0] = -dCos(fAngle);
        cData.avCylinderNormals[i][1] = -dSin(fAngle);
        cData.avCylinderNormals[i][2] = REAL(0.0);

        fAngle += fAngleIncrement;
    }

    dSetZero(cData.vBestPoint,4);
    // reset best depth
    cData.fBestCenter = REAL(0.0);
}
예제 #23
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }

  /*{
    for (int i = 1; i < IndexCount; i++) {
      dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
    }
  }*/

  {const dReal* Pos = dGeomGetPosition(TriMesh);
  const dReal* Rot = dGeomGetRotation(TriMesh);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
	const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
	const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
	dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
  }}}

  if (Ray){
	  dVector3 Origin, Direction;
	  dGeomRayGet(Ray, Origin, Direction);
	  
	  dReal Length = dGeomRayGetLength(Ray);
	  
	  dVector3 End;
	  End[0] = Origin[0] + (Direction[0] * Length);
	  End[1] = Origin[1] + (Direction[1] * Length);
	  End[2] = Origin[2] + (Direction[2] * Length);
	  End[3] = Origin[3] + (Direction[3] * Length);
	  
	  dsDrawLine(Origin, End);
  }
}
예제 #24
0
void TSRODERigidBody::DebugRender()
{
    Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState );
    TSRMatrix4 bodyTransform;
    TSRMatrix4 geomTransform;
    const float* pBodyPosition = dBodyGetPosition( m_BodyID );
    const float* pBodyRotation = dBodyGetRotation( m_BodyID );
    ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation );
    TSRGlobalConstants.PushMatrix();
    TSRGlobalConstants.MultMatrix( bodyTransform.d );
    TSRDebugDraw::RenderAxis( 1.0f );
    TSRDebugDraw::RenderSphere( 0.25f );

    for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
    {
        dGeomID currGeomTransformID = m_GeomIDs[ i ];
        dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID );

        const float* pGeomPosition = dGeomGetPosition( geomID );
        const float* pGeomRotation = dGeomGetRotation( geomID );
        ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation );
        TSRGlobalConstants.PushMatrix();
        TSRGlobalConstants.MultMatrix( bodyTransform.d );
        switch( dGeomGetClass( geomID ) )
        {
        case dBoxClass:
            {
                dVector3 extents;
                dGeomBoxGetLengths( geomID, extents );
                TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] );
                TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] );
                aabbMin *= 0.5f;
                aabbMax *= 0.5f;
                TSRDebugDraw::RenderAABB( aabbMin, aabbMax );
            }
            break;

        case dSphereClass:
            {
                float radius;
                radius = dGeomSphereGetRadius( geomID );
                TSRDebugDraw::RenderSphere( radius );
            }
            break;

        case dCylinderClass:
            {
                float radius,length;
                dGeomCylinderGetParams( geomID, &radius, &length );
                TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) );
            }
            break;
        }
		TSRGlobalConstants.PopMatrix();
    }
    TSRGlobalConstants.PopMatrix();
    Graphics()->SetRasterizerState( Graphics()->m_FillSolidState );
}
예제 #25
0
 Pose Primitive::getPose() const {
   if(!geom) {
     if (!body)
       return Pose::translate(0.0f,0.0f,0.0f); // fixes init bug
     else
       return osgPose(dBodyGetPosition(body), dBodyGetRotation(body));
   }
   return osgPose(dGeomGetPosition(geom), dGeomGetRotation(geom));
 }
예제 #26
0
void Cube::Update()
{
    const dReal *p,*r;
    //UpdateDisableState();
    p = dGeomGetPosition(iGeom);
    iPosition.x = p[0];
    iPosition.y = p[1];
    iPosition.z = p[2];
}
예제 #27
0
void CODEGeom::get_local_center_bt(Fvector& center)
{
	if(! m_geom_transform) return;
	if(!geom())		//geom is not transformed
	{
		center.set(0.f,0.f,0.f);
	}
	center.set(*((const Fvector*)dGeomGetPosition(geom())));
}
예제 #28
0
Vec3f PObject::getPos()
{
    if (isPlaceable())
    {
        const dReal *pos = dGeomGetPosition(geom);
        return Vec3f(pos[0], pos[1], pos[2]);
    }
    else
        return Vec3f(0,0,0);
}
예제 #29
0
 dReal*
 getWheelPosition (uint wheel_id)
 {
   if (wheel_id > robot_wheels_geom.size ())
   {
     //no such wheel
     exit (-1);
   }
   return (dReal*)dGeomGetPosition (robot_wheels_geom[wheel_id]);
 }
예제 #30
0
void TransformedGeometryExtensionLocalParams(dGeomID geom_transform,const dReal* axis,float center_prg,dReal* local_axis,dReal& local_center_prg)
{
	R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform");
	const dReal* rot=dGeomGetRotation(geom_transform);
	const dReal* pos=dGeomGetPosition(geom_transform);
	dVector3	local_pos;

	dMULTIPLY1_331(local_axis,rot,axis);
	dMULTIPLY1_331(local_pos,rot,pos);
	local_center_prg=center_prg-dDOT(local_pos,local_axis);
}