Leadwerks Engine raycast vehicle code

[Leadwerks]

This is BlitzMax code:

Code: Select all

Rem
bbdoc:
EndRem
Function CreateVehicle:TVehicle(body:TBody)
   Return TVehicle.Create(body)
EndFunction

Rem
bbdoc:
EndRem
Function AddVehicleTire:Int(vehicle:TVehicle,position:TVec3,radius:Float,suspensionlength:Float,springconstant:Float=70.0,springdamping:Float=150.0)
   Return vehicle.addtire(position,radius,suspensionlength,springconstant,springdamping)
EndFunction

Rem
bbdoc:
EndRem
Function SetSteerAngle(vehicle:TVehicle,angle:Float,tireindex:Int)
   vehicle.SetSteerAngle(angle,tireindex:Int)
EndFunction

Rem
bbdoc:
EndRem
Function AddTireTorque(vehicle:TVehicle,torque:Float,tireindex:Int=-1)
   vehicle.AddTireTorque(torque,tireindex:Int)
EndFunction

Rem
bbdoc:
EndRem
Function GetTireMatrix:TMat4(vehicle:TVehicle,tireindex:Int)
   Return vehicle.tire[tireindex].GetMatrix()
EndFunction

Rem
bbdoc:
EndRem
Type TVehicle Extends TJoint {expose}
   
   Field tire:TTire[]
   Field link:TLink {hidden}
      
   Method Free()
      For Local n:Int=0 To tire.length-1
         tire[n].Free()
      Next
      tire=Null
      If link
         link.remove()
         link=Null
      EndIf
      Super.free()
   EndMethod
   
   Method AddTire:Int(position:TVec3,radius:Float,suspensionlength:Float,springconstant:Float=70.0,springdamping:Float=150.0)
      tire=tire[..tire.length+1]
      tire[tire.length-1]=New TTire
      tire[tire.length-1].radius=radius
      tire[tire.length-1].position=position.copy()
      tire[tire.length-1].vehicle=Self
      If suspensionlength=0.0 suspensionlength = radius*0.5
      tire[tire.length-1].suspensionlength=suspensionlength
      tire[tire.length-1].springconstant=springconstant
      tire[tire.length-1].springdamping=springdamping
      Return tire.length-1
   EndMethod
   
   Method Update() {hidden}
      If parent.getmass()>0.0
         For Local n:Int=0 To tire.length-1
            tire[n].Update(Self)
         Next
      EndIf
   EndMethod
   
   Method SetSteerAngle(angle:Float,tireindex:Int)
      tire[tireindex].angle=angle
   EndMethod
   
   Method AddTireTorque(torque:Float,tireindex:Int=-1)
      If tireindex=-1
         For Local n:Int=0 To tire.length-1
            tire[n].speed=torque
         Next      
      Else
         tire[tireindex].speed=torque
      EndIf
   EndMethod
   
   Method GetTireMatrix:TMat4(tireindex:Int)
      Return tire[tireindex].getmatrix()
   EndMethod
   
   Method GetTireCollisionEntity:TEntity(tireindex:Int)
      Local pick:TPick=tire[tireindex].pick
      If pick
         Return pick.entity
      Else
         Return Null
      EndIf
   EndMethod
   
   Method GetTire:TTire(tireindex:Int)
      Return tire[tireindex]
   EndMethod
   
   Method TireIsAirborne:Int(tireindex)
      Return tire[tireindex].airborne
   EndMethod
   
   Function Create:TVehicle(body:TBody)
      Local vehicle:TVehicle=New TVehicle
      vehicle.parent=body
      vehicle.ConnectLinks()
      vehicle.link=body.world.vehicles.addfirst(vehicle)
      Return vehicle
   EndFunction
   
EndType

Function TireIsAirborne:Int(vehicle:TVehicle, index:Int)
   Return vehicle.TireIsAirborne(index)
EndFunction

Type TTire {expose}
   
   Global ConvexCastCollisionType:Int
   Global tempentity:TEntity {hidden}
   
   Field position:TVec3=vec3(0)
   Field angle:Float
   Field radius:Float
   Field airborne:Int {hidden}
   Field compression:Float {hidden}
   Field L_comp_prev:Float {hidden}
   Field speed:Float {hidden}
   Field vehicle:TVehicle {hidden}
   Field rotation:Float {hidden}
   Field lastposition:TVec3 {hidden}
   Field brakes:Float {hidden}
   Field ispowered:Int {hidden}
   Field turnspeed:Float {hidden}
   Field maxlateralforce:Float {hidden}
   Field maxlongitudinalforce:Float {hidden}
   Field brakeforce:Float {hidden}
   Field suspensionlength:Float {hidden}
   Field springconstant:Float {hidden}
   Field springdamping:Float {hidden}
   Field contactposition:TVec3 {hidden}
   Field rotationfromtorque:Float {hidden}
   Field rotationfromtorquespeed:Float {hidden}
   Field laterialslippage:Float
   Field longtudinalslippage:Float
   Field pick:TPick
   Field shape:Byte Ptr
   
   Method Free()
      vehicle=Null
      If shape
         NewtonReleaseCollision TWorld.Current.newtonWorld,shape
         shape=Null
      EndIf
   EndMethod
   
   Method GetMatrix:TMat4()
      Local mat:TMat4
      Local p:TEntity = CreatePivot(vehicle.parent)
      p.SetRotation(vec3(0,angle,0))
      p.SetPosition(position)
      p.move(vec3(0,-suspensionlength-L_comp_prev,0))
      p.turn(vec3(rotation+rotationfromtorque,0,0))
      mat=p.mat.copy()
      p.free()
      Return mat
   EndMethod
   
   Method UpdateSuspension()
      Local globaltireposition:TVec3=TFormPointm(position,vehicle.parent.nextmat,Null)
      Local t0:TVec3=globaltireposition.minus(vehicle.parent.nextmat.j().scale(radius))
      Local t1:TVec3=t0.minus(vehicle.parent.nextmat.j().scale(suspensionlength+0.1))
      
      airborne=1
      tempentity=vehicle.parent
      
      Local pickradius#=0.1
      Local mat:TMat4=TMat4.identity()
      mat.ty = pickradius*0.5
      
      If Not shape shape = NewtonCreateBox(TWorld.Current.newtonworld, pickradius, pickradius, pickradius, 0, mat)

      MemCopy mat,vehicle.parent.nextmat,48
      MemCopy Varptr mat.tx,t0,12
      mat.ty:+radius
      
      ConvexCastCollisionType=vehicle.parent.collisiontype
      
      pick=ConvexCast(mat,t1,shape)
            
      tempentity = Null
      
      'Adjust suspension
      If pick      
         airborne=0
         
         RecordCollision(vehicle.parent,pick.entity,pick.position,pick.normal)
         
         'pick.position=TFormPointm(Self.position,Self.vehicle.parent.nextmat,Null)
         
         contactposition=pick.position.copy()
         pick.position = TFormPointm(pick.position,Null,vehicle.parent.nextmat)
         Local lt0:TVec3=TFormPointm( t0,Null,vehicle.parent.nextmat )         
         
         Local d#=lt0.y-pick.position.y
         Local l_comp:Float=d-suspensionlength
         Local k_susp:Float=springconstant
         Local k_damp:Float=springdamping
         Local F_spring:Float
         Local F_damp:Float
         
         d=Clamp(dot(pick.normal,vehicle.parent.nextmat.j()))
         If d<0.5
            l_comp=0.0
            speed=0.0
         Else
            'd=(d-0.75)/0.75
            speed:*d
            l_comp:*d
         EndIf   
         
         F_spring = L_comp * k_susp;
         F_damp = (L_comp - L_comp_prev) * k_damp;
         l_comp=Clamp(l_comp,-suspensionlength,0.0)
         L_comp_prev=l_comp
                  
         Local euler:TVec3=vehicle.parent.nextquat.toeuler()
         euler.x=0
         euler.z=0
         Local temp2:TMat4=TMat4.fromrotation(euler.toquat())
         temp2.tx=vehicle.parent.nextmat.tx
         temp2.ty=vehicle.parent.nextmat.ty
         temp2.tz=vehicle.parent.nextmat.tz
         Local force:TVec3=TFormVectorm(vec3(0,-(F_Spring+F_damp),0),vehicle.parent.nextmat,temp2)
         If 90.0-ASin(pick.normal.y)<45.0
            force.x=0
         Else
            airborne=1
         EndIf
         force = TFormVectorm(force,temp2,Null)
         
         vehicle.parent.AddForceAtPoint( force, t0 )
      Else
         L_comp_prev = 0.0
      EndIf
   EndMethod
   
   Method Update(vehicle:TVehicle)
      Local globaltireposition:TVec3=TFormPoint(position,vehicle.parent,Null)
      
      UpdateSuspension()
      
      'Tire traction
      If Not airborne
         Local q:TQuat=vec3(0,angle,0).toquat().times(vehicle.parent.nextmat.rotation().normalize()).normalize()
         
         q=TFormQuatm(vec3(0,angle,0).toquat(),vehicle.parent.nextmat,Null)
         
         Local p:TMat4=TMat4.FromRotation(q)
         Local v:TVec3
         Local dv:TVec3
         Local temp:Float
         
         p.tx=vehicle.parent.nextmat.tx+position.x
         p.ty=vehicle.parent.nextmat.tx+position.y
         p.tz=vehicle.parent.nextmat.tx+position.z
         
         contactposition=TFormPointm(Self.position,Self.vehicle.parent.nextmat,Null)
         
         v=vehicle.parent.GetVelocityAtPoint(contactposition)
         v=TFormVectorm(v,Null,p)
         'v.x=0.0
         v.x:*-1.0*vehicle.parent.mass
         If maxlateralforce<>0.0
            temp = v.x
            v.x = Min(maxlateralforce,Abs(v.x)) * Sgn(v.x)
            laterialslippage=Abs(temp)-Abs(v.x)
         EndIf
         v.y=0.0
         v.z=speed
         v=TFormVectorm(v,p,Null)
         vehicle.parent.AddForceAtPoint(v, contactposition)
      EndIf
      
      'Tire rotation
      Local dist#
      Local c#=2.0*Pi*radius
      If (airborne<>0 Or longtudinalslippage<>0.0)
         turnspeed :+ (speed / c * 360.0/60.0) * 0.03
         turnspeed = turnspeed * 0.995 + 0.05 * 0.0
         lastposition=Null
      Else
         If lastposition
            lastposition=tformpointm(lastposition,Null,vehicle.parent.nextmat)
            dist = position.z - lastposition.z
            turnspeed = dist / c * 360.0
         EndIf
      EndIf
      rotation :+ turnspeed
      lastposition=TFormPointm(position,vehicle.parent.nextmat,Null)
      
   EndMethod

   Function ConvexCast:TPick(mat:TMat4,b:TVec3,shape:Byte Ptr) {hidden}
      Const maxContactsCount:Int=100
      Global contactdata:Byte[maxContactsCount*SizeOf(New TNewtonWorldConvexCastReturnInfoTag)]
      Global hitParam:Float[maxContactsCount]
      Local tag:TNewtonWorldConvexCastReturnInfoTag=New TNewtonWorldConvexCastReturnInfoTag
      Local result:TPick
      Local contacts:Int
      Local n:Int
      Local maxhitparam:Float
      Local newtonbody:Byte Ptr
      Local entity:TEntity
      Local bias:Float
            
      contacts=NewtonWorldConvexCast(currentworld().newtonWorld,mat,b,shape,hitParam,Null,PreFilter,contactData,maxContactsCount,0)
            
      For n=0 To contacts-1
         MemCopy tag,Varptr contactdata[n*(SizeOf(tag))],SizeOf(tag)
         newtonbody=tag.m_hitBody
         entity=TEntity(NewtonBodyGetUserData(newtonbody))
         If entity
            If entity.getclass()=ENTITY_TERRAIN
               If Not TTerrain(entity).getvisibilityatpoint(tag.point0,tag.point2) Continue
            EndIf
            
            If hitParam[n]<maxhitparam Or n=0
               maxhitparam = hitParam[n]
               If Not result result=New TPick
               result.entity=entity
               result.position.x=tag.point0
               result.position.y=tag.point1
               result.position.z=tag.point2   
               result.normal.x=tag.normal0
               result.normal.y=tag.normal1
               result.normal.z=tag.normal2   
            EndIf
         EndIf   
      Next
      
      Return result
   EndFunction
   
   Function PreFilter:Int(newtonbody:Byte Ptr,newtoncollision:Byte Ptr,userData:Byte Ptr)
      Local entity:TEntity=TEntity(NewtonBodyGetUserData(newtonbody))
      If entity
         If entity=tempentity Return 0
         If Not entity.hidden()
            If GetCollisionResponse(entity.collisiontype,ConvexCastCollisionType)=1
               Return 1
            EndIf
         EndIf
      EndIf
      Return 0
   EndFunction
      
EndType


[ledahut]

Thank you for this code it helped me a lot. (I use delphi/pascal language).
But I still don't understand how you add friction on tire side.
I think its here

Code: Select all

        v=vehicle.parent.GetVelocityAtPoint(contactposition)
         v=TFormVectorm(v,Null,p)
         'v.x=0.0
         v.x:*-1.0*vehicle.parent.mass
         If maxlateralforce<>0.0
            temp = v.x
            v.x = Min(maxlateralforce,Abs(v.x)) * Sgn(v.x)
            laterialslippage=Abs(temp)-Abs(v.x)
         EndIf
         v.y=0.0
         v.z=speed
         v=TFormVectorm(v,p,Null)
         vehicle.parent.AddForceAtPoint(v, contactposition)

-First you get global velocity in parent coordinate system (VX become lateral velocity, VZ become front velocity).
-Then you scale VX by -1*CarMass
-After you get a minimum and save 'laterialslippage'
-Finnaly the code is clear.

Where do you use laterialslippage after that?

[ledahut]

Ok lateral friction work nice.
I was using bad local/global space in my code.

I see that 'longitudinalslippage' allow you set tire rotation (visual representation). Where do you set its value?

Code: Select all

 'Tire rotation
      Local dist#
      Local c#=2.0*Pi*radius
      If (airborne<>0 Or longtudinalslippage<>0.0)
         turnspeed :+ (speed / c * 360.0/60.0) * 0.03
         turnspeed = turnspeed * 0.995 + 0.05 * 0.0
         lastposition=Null
      Else
         If lastposition
            lastposition=tformpointm(lastposition,Null,vehicle.parent.nextmat)
            dist = position.z - lastposition.z
            turnspeed = dist / c * 360.0
         EndIf
      EndIf
      rotation :+ turnspeed
      lastposition=TFormPointm(position,vehicle.parent.nextmat,Null)

Where can I use laterialslippage value from my first post?