~mulder-nebulon/openracing/cleaned

« back to all changes in this revision

Viewing changes to src/ode/OdeSimulator.cs

  • Committer: Nagy Imre
  • Date: 2009-04-04 06:54:16 UTC
  • Revision ID: mulder@codenode-20090404065416-ybb5akhb6im7b0x6
Almost ok, but in Z-180, it rotates backwards

Show diffs side-by-side

added added

removed removed

Lines of Context:
108
108
        // return height of the first polygon point hit by a ray casted down from x,y,z.
109
109
        float RaytraceGroundDown(float x, float y, float z) 
110
110
        {
111
 
//          return 0.0f; // IMRE: for now we do not use ODE
112
 
//          TODO: use ODE instead of Ogre mesh
113
 
//          if (x<=0 || z<=0 || y<=0) return 1.0f;
114
111
            float ret = 0.0f;
115
112
 
116
113
            Ode.dGeomRaySet(mHeightRayId, x, z+1, -y, 0,-1,0);
117
114
            int numContacts = Ode.dCollide(mHeightRayId,  mGroundGeom, mContacts.Length, mContacts, Marshal.SizeOf(mContacts[0]));
118
115
            if (numContacts < 1) 
119
116
                {
120
 
                    Console.Out.WriteLine("ODE-RAYCAST ERROR {0} {1} {2} {3}", x,y,z,Marshal.SizeOf(mContacts[0]));
121
 
                    // ret = mOdeMesh.RaytraceDown(x,z,-y);
 
117
//                  Console.Out.WriteLine("ODE-RAYCAST ERROR {0} {1} {2} {3}", x,y,z,Marshal.SizeOf(mContacts[0]));
122
118
                    ret = 0.0f;
123
119
                }
124
120
            else 
125
121
                {
126
 
                    Console.Out.WriteLine("ODE-SOURCE : {0} {1} {2}                             {3}             {4} {5} {6}", x, z, -y, mContacts[0].pos.Y, mContacts[0].normal.X,   mContacts[0].normal.Y,  mContacts[0].normal.Z );
127
 
//                    Console.Out.WriteLine("ODE-HIT    : {0} {1} {2}", mContacts[0].pos.X, mContacts[0].pos.Y, mContacts[0].pos.Z);
 
122
//                    Console.Out.WriteLine("ODE-SOURCE : {0} {1} {2}                                   {3}             {4} {5} {6}", x, z, -y, mContacts[0].pos.Y, mContacts[0].normal.X,   mContacts[0].normal.Y,  mContacts[0].normal.Z );
128
123
                    ret = mContacts[0].pos.Y;
129
124
                }
130
 
            if (ret < 1.0f) ret = 1.0f;
131
125
            return ret;
132
126
        }
133
127
        
 
128
        
 
129
        //TODO: this function might be integrated into TaytraceGroundDown, so raycast is not called twice
 
130
        Ode.dVector3 RaytraceNormalGroundDown(float x, float y, float z)
 
131
            {
 
132
                Ode.dVector3 retvector = new Ode.dVector3();
 
133
                
 
134
                Ode.dGeomRaySet(mHeightRayId, x, z+10, -y, 0,-1,0);
 
135
                int numContacts = Ode.dCollide(mHeightRayId,  mGroundGeom, mContacts.Length, mContacts, Marshal.SizeOf(mContacts[0]));
 
136
                if (numContacts < 1) 
 
137
                    {
 
138
                        Console.Out.WriteLine("ODE-RAYCAST NORMAL-ERROR {0} {1} {2} {3}", x,y,z,Marshal.SizeOf(mContacts[0]));
 
139
                    }
 
140
                else 
 
141
                    {
 
142
                        retvector = mContacts[0].normal;
 
143
                    }
 
144
                return retvector;
 
145
            }
 
146
        
134
147
        // Compute zRoad for each wheel.
135
148
        void ComputeWheelsZRoad(tSituation situ) {
136
149
            for (int carID = 0; carID < situ.RaceInfo.CarsCount; ++carID) {
137
150
                tWheelStateArray wheelArray = situ.Cars[carID].Priv.Wheels;
138
151
                Math3D.Matrix4 m = situ.Cars[carID].PositionMatrix;
139
 
                for (int wheelID = 0; wheelID < 4; ++wheelID) {
140
 
                    tWheelState wheel = wheelArray[wheelID];
141
 
                    float x = wheel.RelativePosition.X;
142
 
                    float y = wheel.RelativePosition.Y;
143
 
                    float z = wheel.RelativePosition.Z;
144
 
                    Math3D.Vector3 v = new Math3D.Vector3(x,y,z);
145
 
                    v = m * v; // Get position of wheel in world coordinate (checked OK)
146
 
                    wheel.zRoad = RaytraceGroundDown(v.x,v.y,v.z);
147
 
                }
 
152
 
 
153
                for (int wheelID = 0; wheelID < 4; ++wheelID) 
 
154
                    {
 
155
                        tWheelState wheel = wheelArray[wheelID];
 
156
                        float x = wheel.RelativePosition.X;
 
157
                        float y = wheel.RelativePosition.Y;
 
158
                        float z = wheel.RelativePosition.Z;
 
159
 
 
160
                        Math3D.Vector3 v = new Math3D.Vector3(x,y,z);
 
161
                        v = m * v; // Get position of wheel in world coordinate (checked OK)
 
162
                        wheel.zRoad = RaytraceGroundDown(v.x,v.y,v.z);
 
163
                        
 
164
//                      Console.Out.WriteLine(" WHEEL: {0}           {1}    {2}    {3}       {4} ", wheelID, v.x, v.y, v.z, wheel.zRoad);
 
165
                        
 
166
                        
 
167
                        //Ode.dVector3 nnormal = RaytraceNormalGroundDown(v.x, v.y, v.z);
 
168
                        //TODO: the whole normal query and pass should be optimized
 
169
                        //wheel.nnorm_x = nnormal.X;
 
170
                        //wheel.nnorm_y = nnormal.Y;
 
171
                        //wheel.nnorm_z = nnormal.Z;
 
172
 
 
173
/*
 
174
                        if              (wheelID==0)    wheel.zRoad = 0.8f;
 
175
                        else if         (wheelID==1)    wheel.zRoad = 0.8f;
 
176
                        else if         (wheelID==2)    wheel.zRoad = 1.0f;
 
177
                        else if         (wheelID==3)    wheel.zRoad = 0.8f;
 
178
*/
 
179
//                      wheel.zRoad = 0.8f;
 
180
            
 
181
/*
 
182
                        wheel.nnorm_x = 1.0f;
 
183
                        wheel.nnorm_y = 0.0f;
 
184
                        wheel.nnorm_z = 1.0f;
 
185
*/                    
 
186
//                      Console.Out.WriteLine("Returning NORMALS:   {0} {1} {2}", wheel.nnorm_x, wheel.nnorm_y, wheel.nnorm_z); 
 
187
                    }
 
188
            
 
189
//            Console.Out.WriteLine("");
 
190
            
 
191
//            Environment.Exit(0);      
148
192
            }
149
193
        }
150
194